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