Source/Math/XYZ.cpp

   1 /*
   2 Copyright (C) 2003, 2010 - Wolfire Games
   3 Copyright (C) 2010-2016 - 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 #include "Math/XYZ.hpp"
  22
  23 bool PointInTriangle(XYZ* p, XYZ normal, XYZ* p1, XYZ* p2, XYZ* p3)
  24 {
  25     static float u0, u1, u2;
  26     static float v0, v1, v2;
  27     static float a, b;
  28     static float max;
  29     static int i, j;
  30     static bool bInter = 0;
  31     static float pointv[3];
  32     static float p1v[3];
  33     static float p2v[3];
  34     static float p3v[3];
  35     static float normalv[3];
  36
  37     bInter = 0;
  38
  39     pointv[0] = p->x;
  40     pointv[1] = p->y;
  41     pointv[2] = p->z;
  42
  43     p1v[0] = p1->x;
  44     p1v[1] = p1->y;
  45     p1v[2] = p1->z;
  46
  47     p2v[0] = p2->x;
  48     p2v[1] = p2->y;
  49     p2v[2] = p2->z;
  50
  51     p3v[0] = p3->x;
  52     p3v[1] = p3->y;
  53     p3v[2] = p3->z;
  54
  55     normalv[0] = normal.x;
  56     normalv[1] = normal.y;
  57     normalv[2] = normal.z;
  58
  59 #define ABS(X) (((X) < 0.f) ? -(X) : (X))
  60 #define MAX(A, B) (((A) < (B)) ? (B) : (A))
  61     max = MAX(MAX(ABS(normalv[0]), ABS(normalv[1])), ABS(normalv[2]));
  62 #undef MAX
  63     if (max == ABS(normalv[0])) {
  64         i = 1; // y, z
  65         j = 2;
  66     }
  67     if (max == ABS(normalv[1])) {
  68         i = 0; // x, z
  69         j = 2;
  70     }
  71     if (max == ABS(normalv[2])) {
  72         i = 0; // x, y
  73         j = 1;
  74     }
  75 #undef ABS
  76
  77     u0 = pointv[i] - p1v[i];
  78     v0 = pointv[j] - p1v[j];
  79     u1 = p2v[i] - p1v[i];
  80     v1 = p2v[j] - p1v[j];
  81     u2 = p3v[i] - p1v[i];
  82     v2 = p3v[j] - p1v[j];
  83
  84     if (u1 > -1.0e-05f && u1 < 1.0e-05f) { // == 0.0f)
  85         b = u0 / u2;
  86         if (0.0f <= b && b <= 1.0f) {
  87             a = (v0 - b * v2) / v1;
  88             if ((a >= 0.0f) && ((a + b) <= 1.0f)) {
  89                 bInter = 1;
  90             }
  91         }
  92     } else {
  93         b = (v0 * u1 - u0 * v1) / (v2 * u1 - u2 * v1);
  94         if (0.0f <= b && b <= 1.0f) {
  95             a = (u0 - b * u2) / u1;
  96             if ((a >= 0.0f) && ((a + b) <= 1.0f)) {
  97                 bInter = 1;
  98             }
  99         }
 100     }
 101
 102     return bInter;
 103 }
 104
 105 bool LineFacet(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ* p)
 106 {
 107     static float d;
 108     static float denom, mu;
 109     static XYZ n;
 110
 111     //Calculate the parameters for the plane
 112     n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);
 113     n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);
 114     n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);
 115     Normalise(&n);
 116     d = -n.x * pa.x - n.y * pa.y - n.z * pa.z;
 117
 118     //Calculate the position on the line that intersects the plane
 119     denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
 120     if (fabs(denom) < 0.0000001) { // Line and plane don't intersect
 121         return 0;
 122     }
 123     mu = -(d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
 124     p->x = p1.x + mu * (p2.x - p1.x);
 125     p->y = p1.y + mu * (p2.y - p1.y);
 126     p->z = p1.z + mu * (p2.z - p1.z);
 127     if (mu < 0 || mu > 1) { // Intersection not along line segment
 128         return 0;
 129     }
 130
 131     if (!PointInTriangle(p, n, &pa, &pb, &pc)) {
 132         return 0;
 133     }
 134
 135     return 1;
 136 }
 137
 138 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ* p)
 139 {
 140     static float d;
 141     static float denom, mu;
 142     static XYZ n;
 143
 144     //Calculate the parameters for the plane
 145     n.x = (pb.y - pa.y) * (pc.z - pa.z) - (pb.z - pa.z) * (pc.y - pa.y);
 146     n.y = (pb.z - pa.z) * (pc.x - pa.x) - (pb.x - pa.x) * (pc.z - pa.z);
 147     n.z = (pb.x - pa.x) * (pc.y - pa.y) - (pb.y - pa.y) * (pc.x - pa.x);
 148     Normalise(&n);
 149     d = -n.x * pa.x - n.y * pa.y - n.z * pa.z;
 150
 151     //Calculate the position on the line that intersects the plane
 152     denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
 153     if (fabs(denom) < 0.0000001) { // Line and plane don't intersect
 154         return 0;
 155     }
 156     mu = -(d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
 157     p->x = p1.x + mu * (p2.x - p1.x);
 158     p->y = p1.y + mu * (p2.y - p1.y);
 159     p->z = p1.z + mu * (p2.z - p1.z);
 160     if (mu < 0 || mu > 1) { // Intersection not along line segment
 161         return 0;
 162     }
 163
 164     if (!PointInTriangle(p, n, &pa, &pb, &pc)) {
 165         return 0;
 166     }
 167
 168     return 1;
 169 }
 170
 171 float LineFacetd(XYZ p1, XYZ p2, XYZ pa, XYZ pb, XYZ pc, XYZ n, XYZ* p)
 172 {
 173     static float d;
 174     static float denom, mu;
 175
 176     //Calculate the parameters for the plane
 177     d = -n.x * pa.x - n.y * pa.y - n.z * pa.z;
 178
 179     //Calculate the position on the line that intersects the plane
 180     denom = n.x * (p2.x - p1.x) + n.y * (p2.y - p1.y) + n.z * (p2.z - p1.z);
 181     if (fabs(denom) < 0.0000001) { // Line and plane don't intersect
 182         return 0;
 183     }
 184     mu = -(d + n.x * p1.x + n.y * p1.y + n.z * p1.z) / denom;
 185     p->x = p1.x + mu * (p2.x - p1.x);
 186     p->y = p1.y + mu * (p2.y - p1.y);
 187     p->z = p1.z + mu * (p2.z - p1.z);
 188     if (mu < 0 || mu > 1) { // Intersection not along line segment
 189         return 0;
 190     }
 191
 192     if (!PointInTriangle(p, n, &pa, &pb, &pc)) {
 193         return 0;
 194     }
 195     return 1;
 196 }
 197
 198 float LineFacetd(XYZ* p1, XYZ* p2, XYZ* pa, XYZ* pb, XYZ* pc, XYZ* p)
 199 {
 200     static float d;
 201     static float denom, mu;
 202     static XYZ n;
 203
 204     //Calculate the parameters for the plane
 205     n.x = (pb->y - pa->y) * (pc->z - pa->z) - (pb->z - pa->z) * (pc->y - pa->y);
 206     n.y = (pb->z - pa->z) * (pc->x - pa->x) - (pb->x - pa->x) * (pc->z - pa->z);
 207     n.z = (pb->x - pa->x) * (pc->y - pa->y) - (pb->y - pa->y) * (pc->x - pa->x);
 208     Normalise(&n);
 209     d = -n.x * pa->x - n.y * pa->y - n.z * pa->z;
 210
 211     //Calculate the position on the line that intersects the plane
 212     denom = n.x * (p2->x - p1->x) + n.y * (p2->y - p1->y) + n.z * (p2->z - p1->z);
 213     if (fabs(denom) < 0.0000001) { // Line and plane don't intersect
 214         return 0;
 215     }
 216     mu = -(d + n.x * p1->x + n.y * p1->y + n.z * p1->z) / denom;
 217     p->x = p1->x + mu * (p2->x - p1->x);
 218     p->y = p1->y + mu * (p2->y - p1->y);
 219     p->z = p1->z + mu * (p2->z - p1->z);
 220     if (mu < 0 || mu > 1) { // Intersection not along line segment
 221         return 0;
 222     }
 223
 224     if (!PointInTriangle(p, n, pa, pb, pc)) {
 225         return 0;
 226     }
 227     return 1;
 228 }
 229
 230 float LineFacetd(XYZ* p1, XYZ* p2, XYZ* pa, XYZ* pb, XYZ* pc, XYZ* n, XYZ* p)
 231 {
 232     static float d;
 233     static float denom, mu;
 234
 235     //Calculate the parameters for the plane
 236     d = -n->x * pa->x - n->y * pa->y - n->z * pa->z;
 237
 238     //Calculate the position on the line that intersects the plane
 239     denom = n->x * (p2->x - p1->x) + n->y * (p2->y - p1->y) + n->z * (p2->z - p1->z);
 240     if (fabs(denom) < 0.0000001) { // Line and plane don't intersect
 241         return 0;
 242     }
 243     mu = -(d + n->x * p1->x + n->y * p1->y + n->z * p1->z) / denom;
 244     p->x = p1->x + mu * (p2->x - p1->x);
 245     p->y = p1->y + mu * (p2->y - p1->y);
 246     p->z = p1->z + mu * (p2->z - p1->z);
 247     if (mu < 0 || mu > 1) { // Intersection not along line segment
 248         return 0;
 249     }
 250
 251     if (!PointInTriangle(p, *n, pa, pb, pc)) {
 252         return 0;
 253     }
 254     return 1;
 255 }