Source/Math/XYZ.hpp

   1 /*
   2 Copyright (C) 2003, 2010 - Wolfire Games
   3 Copyright (C) 2010-2017 - Lugaru contributors (see AUTHORS file)
   4
   5 This file is part of Lugaru.
   6
   7 Lugaru is free software; you can redistribute it and/or modify
   8 it under the terms of the GNU General Public License as published by
   9 the Free Software Foundation; either version 2 of the License, or
  10 (at your option) any later version.
  11
  12 Lugaru is distributed in the hope that it will be useful,
  13 but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License
  18 along with Lugaru.  If not, see <http://www.gnu.org/licenses/>.
  19 */
  20
  21 #ifndef _QUATERNIONS_HPP_
  22 #define _QUATERNIONS_HPP_
  23
  24 #include "Graphic/gamegl.hpp"
  25
  26 #include <math.h>
  27
  28 class XYZ
  29 {
  30 public:
  31     float x;
  32     float y;
  33     float z;
  34     XYZ()
  35         : x(0.0f)
  36         , y(0.0f)
  37         , z(0.0f)
  38     {
  39     }
  40     inline XYZ operator+(XYZ add);
  41     inline XYZ operator-(XYZ add);
  42     inline XYZ operator*(float add);
  43     inline XYZ operator*(XYZ add);
  44     inline XYZ operator/(float add);
  45     inline void operator+=(XYZ add);
  46     inline void operator-=(XYZ add);
  47     inline void operator*=(float add);
  48     inline void operator*=(XYZ add);
  49     inline void operator/=(float add);
  50     inline void operator=(float add);
  51     inline bool operator==(XYZ add);
  52 };
  53
  54 inline void CrossProduct(XYZ* P, XYZ* Q, XYZ* V);
  55 inline void CrossProduct(XYZ P, XYZ Q, XYZ* V);
  56 inline void Normalise(XYZ* vectory);
  57 inline float normaldotproduct(XYZ point1, XYZ point2);
  58 inline float fast_sqrt(register float arg);
  59 bool PointInTriangle(XYZ* p, XYZ normal, XYZ* p1, XYZ* p2, XYZ* p3);
  60 bool LineFacet(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ* p);
  61 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ* p);
  62 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ n, XYZ* p);
  63 float LineFacetd(XYZ* p1, XYZ* p2, XYZ* pa, XYZ* pb, XYZ* pc, XYZ* n, XYZ* p);
  64 float LineFacetd(XYZ* p1, XYZ* p2, XYZ* pa, XYZ* pb, XYZ* pc, XYZ* p);
  65 inline void ReflectVector(XYZ* vel, const XYZ* n);
  66 inline void ReflectVector(XYZ* vel, const XYZ& n);
  67 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang);
  68 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang);
  69 inline float findDistance(XYZ* point1, XYZ* point2);
  70 inline float findLength(XYZ* point1);
  71 inline float findLengthfast(XYZ* point1);
  72 inline float distsq(XYZ* point1, XYZ* point2);
  73 inline float distsq(XYZ point1, XYZ point2);
  74 inline float distsqflat(XYZ* point1, XYZ* point2);
  75 inline float dotproduct(const XYZ* point1, const XYZ* point2);
  76 bool sphere_line_intersection(
  77     float x1, float y1, float z1,
  78     float x2, float y2, float z2,
  79     float x3, float y3, float z3, float r);
  80 bool sphere_line_intersection(
  81     XYZ* p1, XYZ* p2, XYZ* p3, float* r);
  82 inline bool DistancePointLine(XYZ* Point, XYZ* LineStart, XYZ* LineEnd, float* Distance, XYZ* Intersection);
  83
  84 inline void Normalise(XYZ* vectory)
  85 {
  86     static float d;
  87     d = fast_sqrt(vectory->x * vectory->x + vectory->y * vectory->y + vectory->z * vectory->z);
  88     if (d == 0) {
  89         return;
  90     }
  91     vectory->x /= d;
  92     vectory->y /= d;
  93     vectory->z /= d;
  94 }
  95
  96 inline XYZ XYZ::operator+(XYZ add)
  97 {
  98     static XYZ ne;
  99     ne = add;
 100     ne.x += x;
 101     ne.y += y;
 102     ne.z += z;
 103     return ne;
 104 }
 105
 106 inline XYZ XYZ::operator-(XYZ add)
 107 {
 108     static XYZ ne;
 109     ne = add;
 110     ne.x = x - ne.x;
 111     ne.y = y - ne.y;
 112     ne.z = z - ne.z;
 113     return ne;
 114 }
 115
 116 inline XYZ XYZ::operator*(float add)
 117 {
 118     static XYZ ne;
 119     ne.x = x * add;
 120     ne.y = y * add;
 121     ne.z = z * add;
 122     return ne;
 123 }
 124
 125 inline XYZ XYZ::operator*(XYZ add)
 126 {
 127     static XYZ ne;
 128     ne.x = x * add.x;
 129     ne.y = y * add.y;
 130     ne.z = z * add.z;
 131     return ne;
 132 }
 133
 134 inline XYZ XYZ::operator/(float add)
 135 {
 136     static XYZ ne;
 137     ne.x = x / add;
 138     ne.y = y / add;
 139     ne.z = z / add;
 140     return ne;
 141 }
 142
 143 inline void XYZ::operator+=(XYZ add)
 144 {
 145     x += add.x;
 146     y += add.y;
 147     z += add.z;
 148 }
 149
 150 inline void XYZ::operator-=(XYZ add)
 151 {
 152     x = x - add.x;
 153     y = y - add.y;
 154     z = z - add.z;
 155 }
 156
 157 inline void XYZ::operator*=(float add)
 158 {
 159     x = x * add;
 160     y = y * add;
 161     z = z * add;
 162 }
 163
 164 inline void XYZ::operator*=(XYZ add)
 165 {
 166     x = x * add.x;
 167     y = y * add.y;
 168     z = z * add.z;
 169 }
 170
 171 inline void XYZ::operator/=(float add)
 172 {
 173     x = x / add;
 174     y = y / add;
 175     z = z / add;
 176 }
 177
 178 inline void XYZ::operator=(float add)
 179 {
 180     x = add;
 181     y = add;
 182     z = add;
 183 }
 184
 185 inline bool XYZ::operator==(XYZ add)
 186 {
 187     if (x == add.x && y == add.y && z == add.z)
 188         return 1;
 189     return 0;
 190 }
 191
 192 inline void CrossProduct(XYZ* P, XYZ* Q, XYZ* V)
 193 {
 194     V->x = P->y * Q->z - P->z * Q->y;
 195     V->y = P->z * Q->x - P->x * Q->z;
 196     V->z = P->x * Q->y - P->y * Q->x;
 197 }
 198
 199 inline void CrossProduct(XYZ P, XYZ Q, XYZ* V)
 200 {
 201     V->x = P.y * Q.z - P.z * Q.y;
 202     V->y = P.z * Q.x - P.x * Q.z;
 203     V->z = P.x * Q.y - P.y * Q.x;
 204 }
 205
 206 inline float fast_sqrt(register float arg)
 207 {
 208     return sqrtf(arg);
 209 }
 210
 211 inline float normaldotproduct(XYZ point1, XYZ point2)
 212 {
 213     static GLfloat returnvalue;
 214     Normalise(&point1);
 215     Normalise(&point2);
 216     returnvalue = (point1.x * point2.x + point1.y * point2.y + point1.z * point2.z);
 217     return returnvalue;
 218 }
 219
 220 inline void ReflectVector(XYZ* vel, const XYZ* n)
 221 {
 222     ReflectVector(vel, *n);
 223 }
 224
 225 inline void ReflectVector(XYZ* vel, const XYZ& n)
 226 {
 227     static XYZ vn;
 228     static XYZ vt;
 229     static float dotprod;
 230
 231     dotprod = dotproduct(&n, vel);
 232     vn.x = n.x * dotprod;
 233     vn.y = n.y * dotprod;
 234     vn.z = n.z * dotprod;
 235
 236     vt.x = vel->x - vn.x;
 237     vt.y = vel->y - vn.y;
 238     vt.z = vel->z - vn.z;
 239
 240     vel->x = vt.x - vn.x;
 241     vel->y = vt.y - vn.y;
 242     vel->z = vt.z - vn.z;
 243 }
 244
 245 inline float dotproduct(const XYZ* point1, const XYZ* point2)
 246 {
 247     static GLfloat returnvalue;
 248     returnvalue = (point1->x * point2->x + point1->y * point2->y + point1->z * point2->z);
 249     return returnvalue;
 250 }
 251
 252 inline float findDistance(XYZ* point1, XYZ* point2)
 253 {
 254     return (fast_sqrt((point1->x - point2->x) * (point1->x - point2->x) + (point1->y - point2->y) * (point1->y - point2->y) + (point1->z - point2->z) * (point1->z - point2->z)));
 255 }
 256
 257 inline float findLength(XYZ* point1)
 258 {
 259     return (fast_sqrt((point1->x) * (point1->x) + (point1->y) * (point1->y) + (point1->z) * (point1->z)));
 260 }
 261
 262 inline float findLengthfast(XYZ* point1)
 263 {
 264     return ((point1->x) * (point1->x) + (point1->y) * (point1->y) + (point1->z) * (point1->z));
 265 }
 266
 267 inline float distsq(XYZ* point1, XYZ* point2)
 268 {
 269     return ((point1->x - point2->x) * (point1->x - point2->x) + (point1->y - point2->y) * (point1->y - point2->y) + (point1->z - point2->z) * (point1->z - point2->z));
 270 }
 271
 272 inline float distsq(XYZ point1, XYZ point2)
 273 {
 274     return ((point1.x - point2.x) * (point1.x - point2.x) + (point1.y - point2.y) * (point1.y - point2.y) + (point1.z - point2.z) * (point1.z - point2.z));
 275 }
 276
 277 inline float distsqflat(XYZ* point1, XYZ* point2)
 278 {
 279     return ((point1->x - point2->x) * (point1->x - point2->x) + (point1->z - point2->z) * (point1->z - point2->z));
 280 }
 281
 282 inline XYZ DoRotation(XYZ thePoint, float xang, float yang, float zang)
 283 {
 284     static XYZ newpoint;
 285     if (xang) {
 286         xang *= 6.283185f;
 287         xang /= 360;
 288     }
 289     if (yang) {
 290         yang *= 6.283185f;
 291         yang /= 360;
 292     }
 293     if (zang) {
 294         zang *= 6.283185f;
 295         zang /= 360;
 296     }
 297
 298     if (yang) {
 299         newpoint.z = thePoint.z * cosf(yang) - thePoint.x * sinf(yang);
 300         newpoint.x = thePoint.z * sinf(yang) + thePoint.x * cosf(yang);
 301         thePoint.z = newpoint.z;
 302         thePoint.x = newpoint.x;
 303     }
 304
 305     if (zang) {
 306         newpoint.x = thePoint.x * cosf(zang) - thePoint.y * sinf(zang);
 307         newpoint.y = thePoint.y * cosf(zang) + thePoint.x * sinf(zang);
 308         thePoint.x = newpoint.x;
 309         thePoint.y = newpoint.y;
 310     }
 311
 312     if (xang) {
 313         newpoint.y = thePoint.y * cosf(xang) - thePoint.z * sinf(xang);
 314         newpoint.z = thePoint.y * sinf(xang) + thePoint.z * cosf(xang);
 315         thePoint.z = newpoint.z;
 316         thePoint.y = newpoint.y;
 317     }
 318
 319     return thePoint;
 320 }
 321
 322 inline float square(float f)
 323 {
 324     return (f * f);
 325 }
 326
 327 inline bool sphere_line_intersection(
 328     float x1, float y1, float z1,
 329     float x2, float y2, float z2,
 330     float x3, float y3, float z3, float r)
 331 {
 332
 333     // x1,y1,z1  P1 coordinates (point of line)
 334     // x2,y2,z2  P2 coordinates (point of line)
 335     // x3,y3,z3, r  P3 coordinates and radius (sphere)
 336     // x,y,z   intersection coordinates
 337     //
 338     // This function returns a pointer array which first index indicates
 339     // the number of intersection point, followed by coordinate pairs.
 340
 341     //~ static float x , y , z;
 342     static float a, b, c, /*mu,*/ i;
 343
 344     if (x1 > x3 + r && x2 > x3 + r)
 345         return (0);
 346     if (x1 < x3 - r && x2 < x3 - r)
 347         return (0);
 348     if (y1 > y3 + r && y2 > y3 + r)
 349         return (0);
 350     if (y1 < y3 - r && y2 < y3 - r)
 351         return (0);
 352     if (z1 > z3 + r && z2 > z3 + r)
 353         return (0);
 354     if (z1 < z3 - r && z2 < z3 - r)
 355         return (0);
 356     a = square(x2 - x1) + square(y2 - y1) + square(z2 - z1);
 357     b = 2 * ((x2 - x1) * (x1 - x3) + (y2 - y1) * (y1 - y3) + (z2 - z1) * (z1 - z3));
 358     c = square(x3) + square(y3) +
 359         square(z3) + square(x1) +
 360         square(y1) + square(z1) -
 361         2 * (x3 * x1 + y3 * y1 + z3 * z1) - square(r);
 362     i = b * b - 4 * a * c;
 363
 364     if (i < 0.0) {
 365         // no intersection
 366         return (0);
 367     }
 368     return (1);
 369 }
 370
 371 inline bool sphere_line_intersection(
 372     XYZ* p1, XYZ* p2, XYZ* p3, float* r)
 373 {
 374
 375     // x1,p1->y,p1->z  P1 coordinates (point of line)
 376     // p2->x,p2->y,p2->z  P2 coordinates (point of line)
 377     // p3->x,p3->y,p3->z, r  P3 coordinates and radius (sphere)
 378     // x,y,z   intersection coordinates
 379     //
 380     // This function returns a pointer array which first index indicates
 381     // the number of intersection point, followed by coordinate pairs.
 382
 383     //~ static float x , y , z;
 384     static float a, b, c, /*mu,*/ i;
 385
 386     if (p1->x > p3->x + *r && p2->x > p3->x + *r)
 387         return (0);
 388     if (p1->x < p3->x - *r && p2->x < p3->x - *r)
 389         return (0);
 390     if (p1->y > p3->y + *r && p2->y > p3->y + *r)
 391         return (0);
 392     if (p1->y < p3->y - *r && p2->y < p3->y - *r)
 393         return (0);
 394     if (p1->z > p3->z + *r && p2->z > p3->z + *r)
 395         return (0);
 396     if (p1->z < p3->z - *r && p2->z < p3->z - *r)
 397         return (0);
 398     a = square(p2->x - p1->x) + square(p2->y - p1->y) + square(p2->z - p1->z);
 399     b = 2 * ((p2->x - p1->x) * (p1->x - p3->x) + (p2->y - p1->y) * (p1->y - p3->y) + (p2->z - p1->z) * (p1->z - p3->z));
 400     c = square(p3->x) + square(p3->y) +
 401         square(p3->z) + square(p1->x) +
 402         square(p1->y) + square(p1->z) -
 403         2 * (p3->x * p1->x + p3->y * p1->y + p3->z * p1->z) - square(*r);
 404     i = b * b - 4 * a * c;
 405
 406     if (i < 0.0) {
 407         // no intersection
 408         return (0);
 409     }
 410     return (1);
 411 }
 412
 413 inline XYZ DoRotationRadian(XYZ thePoint, float xang, float yang, float zang)
 414 {
 415     static XYZ newpoint;
 416     static XYZ oldpoint;
 417
 418     oldpoint = thePoint;
 419
 420     if (yang != 0) {
 421         newpoint.z = oldpoint.z * cosf(yang) - oldpoint.x * sinf(yang);
 422         newpoint.x = oldpoint.z * sinf(yang) + oldpoint.x * cosf(yang);
 423         oldpoint.z = newpoint.z;
 424         oldpoint.x = newpoint.x;
 425     }
 426
 427     if (zang != 0) {
 428         newpoint.x = oldpoint.x * cosf(zang) - oldpoint.y * sinf(zang);
 429         newpoint.y = oldpoint.y * cosf(zang) + oldpoint.x * sinf(zang);
 430         oldpoint.x = newpoint.x;
 431         oldpoint.y = newpoint.y;
 432     }
 433
 434     if (xang != 0) {
 435         newpoint.y = oldpoint.y * cosf(xang) - oldpoint.z * sinf(xang);
 436         newpoint.z = oldpoint.y * sinf(xang) + oldpoint.z * cosf(xang);
 437         oldpoint.z = newpoint.z;
 438         oldpoint.y = newpoint.y;
 439     }
 440
 441     return oldpoint;
 442 }
 443
 444 inline bool DistancePointLine(XYZ* Point, XYZ* LineStart, XYZ* LineEnd, float* Distance, XYZ* Intersection)
 445 {
 446     float LineMag;
 447     float U;
 448
 449     LineMag = findDistance(LineEnd, LineStart);
 450
 451     U = (((Point->x - LineStart->x) * (LineEnd->x - LineStart->x)) +
 452          ((Point->y - LineStart->y) * (LineEnd->y - LineStart->y)) +
 453          ((Point->z - LineStart->z) * (LineEnd->z - LineStart->z))) /
 454         (LineMag * LineMag);
 455
 456     if (U < 0.0f || U > 1.0f)
 457         return 0; // closest point does not fall within the line segment
 458
 459     Intersection->x = LineStart->x + U * (LineEnd->x - LineStart->x);
 460     Intersection->y = LineStart->y + U * (LineEnd->y - LineStart->y);
 461     Intersection->z = LineStart->z + U * (LineEnd->z - LineStart->z);
 462
 463     *Distance = findDistance(Point, Intersection);
 464
 465     return 1;
 466 }
 467
 468 #endif