summaryrefslogtreecommitdiffhomepage
path: root/nGenEx/Geometry.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'nGenEx/Geometry.cpp')
-rw-r--r--nGenEx/Geometry.cpp1420
1 files changed, 722 insertions, 698 deletions
diff --git a/nGenEx/Geometry.cpp b/nGenEx/Geometry.cpp
index e3dc9b1..74d6f53 100644
--- a/nGenEx/Geometry.cpp
+++ b/nGenEx/Geometry.cpp
@@ -1,698 +1,722 @@
-/* Project nGenEx
- Destroyer Studios LLC
- Copyright © 1997-2004. All Rights Reserved.
-
- SUBSYSTEM: nGenEx.lib
- FILE: Geometry.cpp
- AUTHOR: John DiCamillo
-
-
- OVERVIEW
- ========
- Geometric Utilities
-*/
-
-#include "MemDebug.h"
-#include "Geometry.h"
-
-// +--------------------------------------------------------------------+
-
-void Rect::Inflate(int dx, int dy)
-{
- x -= dx;
- w += dx*2;
- y -= dy;
- h += dy*2;
-}
-
-void Rect::Deflate(int dx, int dy)
-{
- x += dx;
- w -= dx*2;
- y += dy;
- h -= dy*2;
-}
-
-void Rect::Inset(int l, int r, int t, int b)
-{
- x += l;
- y += t;
- w -= l + r;
- h -= t + b;
-}
-
-int Rect::Contains(int ax, int ay) const
-{
- if (ax < x) return 0;
- if (ax > x+w) return 0;
- if (ay < y) return 0;
- if (ay > y+h) return 0;
-
- return 1;
-}
-
-// +--------------------------------------------------------------------+
-
-double
-Point::Normalize()
-{
- double scale = 1.0;
- double len = length();
-
- if (len)
- scale /= len;
-
- x *= scale;
- y *= scale;
- z *= scale;
-
- return len;
-}
-
-// +--------------------------------------------------------------------+
-
-void
-Point::SetElement(int i, double v)
-{
- switch (i) {
- case 0: x = v; break;
- case 1: y = v; break;
- case 2: z = v; break;
- default: break;
- }
-}
-
-// +--------------------------------------------------------------------+
-
-Point
-Point::operator*(const Matrix& m) const
-{
- Point result;
-
- result.x = (m.elem[0][0] * x) + (m.elem[1][0] * y) + (m.elem[2][0] * z);
- result.y = (m.elem[0][1] * x) + (m.elem[1][1] * y) + (m.elem[2][1] * z);
- result.z = (m.elem[0][2] * x) + (m.elem[1][2] * y) + (m.elem[2][2] * z);
-
- return result;
-}
-
-// +--------------------------------------------------------------------+
-
-double ClosestApproachTime(const Point& loc1, const Point& vel1,
-const Point& loc2, const Point& vel2)
-{
- double t = 0;
-
- Point D = loc1-loc2;
- Point Dv = vel1-vel2;
-
- if (Dv.x || Dv.y || Dv.z)
- t = -1 * (Dv*D) / (Dv*Dv);
-
- return t;
-}
-
-// +--------------------------------------------------------------------+
-
-float
-Vec2::Normalize()
-{
- float scale = 1.0f;
- float len = length();
-
- if (len)
- scale /= len;
-
- x *= scale;
- y *= scale;
-
- return len;
-}
-
-// +--------------------------------------------------------------------+
-
-float
-Vec3::Normalize()
-{
- float scale = 1.0f;
- float len = length();
-
- if (len)
- scale /= len;
-
- x *= scale;
- y *= scale;
- z *= scale;
-
- return len;
-}
-
-// +--------------------------------------------------------------------+
-
-Vec3
-Vec3::operator*(const Matrix& m) const
-{
- Vec3 result;
-
- result.x = (float) ((m.elem[0][0] * x) + (m.elem[1][0] * y) + (m.elem[2][0] * z));
- result.y = (float) ((m.elem[0][1] * x) + (m.elem[1][1] * y) + (m.elem[2][1] * z));
- result.z = (float) ((m.elem[0][2] * x) + (m.elem[1][2] * y) + (m.elem[2][2] * z));
-
- return result;
-}
-
-// +--------------------------------------------------------------------+
-
-double ClosestApproachTime(const Vec3& loc1, const Vec3& vel1,
-const Vec3& loc2, const Vec3& vel2)
-{
- double t = 0;
-
- Point D = loc1-loc2;
- Point Dv = vel1-vel2;
-
- if (Dv.x || Dv.y || Dv.z)
- t = -1 * (Dv*D) / (Dv*Dv);
-
- return t;
-}
-
-// +--------------------------------------------------------------------+
-
-double
-Quaternion::Normalize()
-{
- double scale = 1.0;
- double len = length();
-
- if (len)
- scale /= len;
-
- x *= scale;
- y *= scale;
- z *= scale;
- w *= scale;
-
- return len;
-}
-
-// +--------------------------------------------------------------------+
-
-Matrix::Matrix()
-{
- Identity();
-}
-
-Matrix::Matrix(const Matrix& m)
-{
- CopyMemory(elem, m.elem, sizeof(elem));
-}
-
-Matrix::Matrix(const Point& vrt, const Point& vup, const Point& vpn)
-{
- elem[0][0] = vrt.x;
- elem[0][1] = vrt.y;
- elem[0][2] = vrt.z;
-
- elem[1][0] = vup.x;
- elem[1][1] = vup.y;
- elem[1][2] = vup.z;
-
- elem[2][0] = vpn.x;
- elem[2][1] = vpn.y;
- elem[2][2] = vpn.z;
-}
-
-// +--------------------------------------------------------------------+
-
-Matrix&
-Matrix::operator =(const Matrix& m)
-{
- CopyMemory(elem, m.elem, sizeof(elem));
-
- return *this;
-}
-
-// +--------------------------------------------------------------------+
-
-Matrix&
-Matrix::operator*=(const Matrix& m)
-{
- return *this = *this * m;
-}
-
-// +--------------------------------------------------------------------+
-
-void
-Matrix::Identity()
-{
- elem[0][0] = 1;
- elem[0][1] = 0;
- elem[0][2] = 0;
-
- elem[1][0] = 0;
- elem[1][1] = 1;
- elem[1][2] = 0;
-
- elem[2][0] = 0;
- elem[2][1] = 0;
- elem[2][2] = 1;
-}
-
-// +--------------------------------------------------------------------+
-
-inline void swap_elem(double& a, double& b) { double t=a; a=b; b=t; }
-
-void
-Matrix::Transpose()
-{
- swap_elem(elem[0][1], elem[1][0]);
- swap_elem(elem[0][2], elem[2][0]);
- swap_elem(elem[1][2], elem[2][1]);
-}
-
-// +--------------------------------------------------------------------+
-
-void
-Matrix::Rotate(double roll, double pitch, double yaw)
-{
- double e[3][3];
- CopyMemory(e, elem, sizeof(elem));
-
- double sr = sin(roll);
- double cr = cos(roll);
- double sp = sin(pitch);
- double cp = cos(pitch);
- double sy = sin(yaw);
- double cy = cos(yaw);
-
- double a,b,c;
-
- a = cy*cr;
- b = cy*sr;
- c = -sy;
-
- elem[0][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
- elem[0][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
- elem[0][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];
-
- a = cp*-sr + sp*sy*cr;
- b = cp* cr + sp*sy*sr;
- c = sp*cy;
-
- elem[1][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
- elem[1][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
- elem[1][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];
-
- a = -sp*-sr + cp*sy*cr;
- b = -sp* cr + cp*sy*sr;
- c = cp*cy;
-
- elem[2][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
- elem[2][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
- elem[2][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];
-}
-
-// +--------------------------------------------------------------------+
-
-void
-Matrix::Roll(double roll)
-{
- double s = sin(roll);
- double c = cos(roll);
-
- double e00 = elem[0][0];
- double e01 = elem[0][1];
- double e02 = elem[0][2];
- double e10 = elem[1][0];
- double e11 = elem[1][1];
- double e12 = elem[1][2];
-
- elem[0][0] = c*e00 + s*e10;
- elem[0][1] = c*e01 + s*e11;
- elem[0][2] = c*e02 + s*e12;
-
- elem[1][0] = -s*e00 + c*e10;
- elem[1][1] = -s*e01 + c*e11;
- elem[1][2] = -s*e02 + c*e12;
-}
-
-// +--------------------------------------------------------------------+
-
-void
-Matrix::Pitch(double pitch)
-{
- double s = sin(pitch);
- double c = cos(pitch);
-
- double e10 = elem[1][0];
- double e11 = elem[1][1];
- double e12 = elem[1][2];
- double e20 = elem[2][0];
- double e21 = elem[2][1];
- double e22 = elem[2][2];
-
- elem[1][0] = c*e10 + s*e20;
- elem[1][1] = c*e11 + s*e21;
- elem[1][2] = c*e12 + s*e22;
-
- elem[2][0] = -s*e10 + c*e20;
- elem[2][1] = -s*e11 + c*e21;
- elem[2][2] = -s*e12 + c*e22;
-}
-
-// +--------------------------------------------------------------------+
-
-void
-Matrix::Yaw(double yaw)
-{
- double s = sin(yaw);
- double c = cos(yaw);
-
- double e00 = elem[0][0];
- double e01 = elem[0][1];
- double e02 = elem[0][2];
- double e20 = elem[2][0];
- double e21 = elem[2][1];
- double e22 = elem[2][2];
-
- elem[0][0] = c*e00 - s*e20;
- elem[0][1] = c*e01 - s*e21;
- elem[0][2] = c*e02 - s*e22;
-
- elem[2][0] = s*e00 + c*e20;
- elem[2][1] = s*e01 + c*e21;
- elem[2][2] = s*e02 + c*e22;
-}
-
-// +--------------------------------------------------------------------+
-
-inline int sign(double d) { return (d >= 0); }
-
-void
-Matrix::ComputeEulerAngles(double& roll, double& pitch, double& yaw) const
-{
- double cy;
-
- yaw = asin(-elem[0][2]);
- cy = cos(yaw);
- roll = asin(elem[0][1] / cy);
- pitch = asin(elem[1][2] / cy);
-
- if (sign(cos(roll)*cy) != sign(elem[0][0]))
- roll = PI - roll;
-
- if (sign(cos(pitch)*cy) != sign(elem[2][2]))
- pitch = PI - pitch;
-}
-
-// +--------------------------------------------------------------------+
-
-Matrix
-Matrix::operator*(const Matrix& m) const
-{
- Matrix r;
-
- r.elem[0][0] = elem[0][0]*m.elem[0][0] + elem[0][1]*m.elem[1][0] + elem[0][2]*m.elem[2][0];
- r.elem[0][1] = elem[0][0]*m.elem[0][1] + elem[0][1]*m.elem[1][1] + elem[0][2]*m.elem[2][1];
- r.elem[0][2] = elem[0][0]*m.elem[0][2] + elem[0][1]*m.elem[1][2] + elem[0][2]*m.elem[2][2];
-
- r.elem[1][0] = elem[1][0]*m.elem[0][0] + elem[1][1]*m.elem[1][0] + elem[1][2]*m.elem[2][0];
- r.elem[1][1] = elem[1][0]*m.elem[0][1] + elem[1][1]*m.elem[1][1] + elem[1][2]*m.elem[2][1];
- r.elem[1][2] = elem[1][0]*m.elem[0][2] + elem[1][1]*m.elem[1][2] + elem[1][2]*m.elem[2][2];
-
- r.elem[2][0] = elem[2][0]*m.elem[0][0] + elem[2][1]*m.elem[1][0] + elem[2][2]*m.elem[2][0];
- r.elem[2][1] = elem[2][0]*m.elem[0][1] + elem[2][1]*m.elem[1][1] + elem[2][2]*m.elem[2][1];
- r.elem[2][2] = elem[2][0]*m.elem[0][2] + elem[2][1]*m.elem[1][2] + elem[2][2]*m.elem[2][2];
-
- return r;
-}
-
-// +--------------------------------------------------------------------+
-
-Point
-Matrix::operator*(const Point& p) const
-{
- Point result;
-
- result.x = (elem[0][0] * p.x) + (elem[0][1] * p.y) + (elem[0][2] * p.z);
- result.y = (elem[1][0] * p.x) + (elem[1][1] * p.y) + (elem[1][2] * p.z);
- result.z = (elem[2][0] * p.x) + (elem[2][1] * p.y) + (elem[2][2] * p.z);
-
- return result;
-}
-
-// +--------------------------------------------------------------------+
-
-Vec3
-Matrix::operator*(const Vec3& v) const
-{
- Vec3 result;
-
- result.x = (float) ((elem[0][0] * v.x) + (elem[0][1] * v.y) + (elem[0][2] * v.z));
- result.y = (float) ((elem[1][0] * v.x) + (elem[1][1] * v.y) + (elem[1][2] * v.z));
- result.z = (float) ((elem[2][0] * v.x) + (elem[2][1] * v.y) + (elem[2][2] * v.z));
-
- return result;
-}
-
-// +--------------------------------------------------------------------+
-
-double
-Matrix::Cofactor(int i, int j) const
-{
- int i1=0;
- int i2=2;
- int j1=0;
- int j2=2;
-
- if (i==0) i1=1; else if (i==2) i2=1;
- if (j==0) j1=1; else if (j==2) j2=1;
-
- double factor = elem[i1][j1]*elem[i2][j2] - elem[i1][j2]*elem[i2][j1];
-
- if ((i+j) & 1)
- factor *= -1;
-
- return factor;
-}
-
-// +--------------------------------------------------------------------+
-
-void
-Matrix::Invert()
-{
- double f[3][3];
- int i, j;
-
- for (i = 0; i < 3; i++)
- for (j = 0; j < 3; j++)
- f[i][j] = Cofactor(j,i);
-
- double det = elem[0][0] * f[0][0] +
- elem[0][1] * f[1][0] +
- elem[0][2] * f[2][0];
-
- if (det != 0) {
- double inv = 1/det;
-
- for (i = 0; i < 3; i++)
- for (j = 0; j < 3; j++)
- elem[i][j] = f[i][j] * inv;
- }
-}
-
-// +--------------------------------------------------------------------+
-// +--------------------------------------------------------------------+
-// +--------------------------------------------------------------------+
-
-Plane::Plane()
-: distance(0.0f)
-{ }
-
-Plane::Plane(const Point& p0, const Point& p1, const Point& p2)
-{
- Point d1 = p1 - p0;
- Point d2 = p2 - p0;
-
- normal = (Vec3) d1.cross(d2);
- normal.Normalize();
-
- distance = (float) (normal * p0);
-}
-
-Plane::Plane(const Vec3& v0, const Vec3& v1, const Vec3& v2)
-{
- Vec3 d1 = v1 - v0;
- Vec3 d2 = v2 - v0;
-
- normal = d1.cross(d2);
- normal.Normalize();
-
- distance = normal * v0;
-}
-
-void Plane::Rotate(const Vec3& v0, const Matrix& m)
-{
- normal = normal * m;
- distance = normal * v0;
-}
-
-void Plane::Translate(const Vec3& v0)
-{
- distance = normal * v0;
-}
-
-// +--------------------------------------------------------------------+
-// 3-D dot product.
-
-double DotProduct(const Point& a, const Point& b)
-{
- return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
-}
-
-// +--------------------------------------------------------------------+
-// 3-D cross product.
-
-void CrossProduct(const Point& a, const Point& b, Point& out)
-{
- out.x = (a.y * b.z) - (a.z * b.y);
- out.y = (a.z * b.x) - (a.x * b.z);
- out.z = (a.x * b.y) - (a.y * b.x);
-}
-
-// +--------------------------------------------------------------------+
-// Concatenate two 3x3 matrices.
-
-void MConcat(double in1[3][3], double in2[3][3], double out[3][3])
-{
- int i, j;
-
- for (i=0 ; i<3 ; i++) {
- for (j=0 ; j<3 ; j++) {
- out[i][j] = in1[i][0] * in2[0][j] +
- in1[i][1] * in2[1][j] +
- in1[i][2] * in2[2][j];
- }
- }
-}
-
-/* GRAPHICS GEMS II ----------------------------------------------------
-*
-* lines_intersect: AUTHOR: Mukesh Prasad
-*
-* This function computes whether two line segments,
-* respectively joining the input points (x1,y1) -- (x2,y2)
-* and the input points (x3,y3) -- (x4,y4) intersect.
-* If the lines intersect, the output variables x, y are
-* set to coordinates of the point of intersection.
-*
-* All values are in integers. The returned value is rounded
-* to the nearest integer point.
-*
-* If non-integral grid points are relevant, the function
-* can easily be transformed by substituting floating point
-* calculations instead of integer calculations.
-*
-* Entry
-* x1, y1, x2, y2 Coordinates of endpoints of one segment.
-* x3, y3, x4, y4 Coordinates of endpoints of other segment.
-*
-* Exit
-* x, y Coordinates of intersection point.
-*
-* The value returned by the function is one of:
-*
-* DONT_INTERSECT 0
-* DO_INTERSECT 1
-* COLLINEAR 2
-*
-* Error conditions:
-*
-* Depending upon the possible ranges, and particularly on 16-bit
-* computers, care should be taken to protect from overflow.
-*
-* In the following code, 'long' values have been used for this
-* purpose, instead of 'int'.
-*
-*/
-
-#define DONT_INTERSECT 0
-#define DO_INTERSECT 1
-#define COLLINEAR 2
-
-inline int SAME_SIGNS(double a, double b)
-{
- return ((a>=0 && b>=0) || (a<0 && b<0));
-}
-
-int
-lines_intersect(
-/* 1st line segment */ double x1, double y1, double x2, double y2,
-/* 2nd line segment */ double x3, double y3, double x4, double y4,
-/* pt of intersect */ double& ix, double& iy)
-{
- double a1, a2, b1, b2, c1, c2; /* Coefficients of line eqns. */
- double r1, r2, r3, r4; /* 'Sign' values */
- double denom, offset, num; /* Intermediate values */
-
- /* Compute a1, b1, c1, where line joining points 1 and 2
- * is "a1 x + b1 y + c1 = 0". */
-
- a1 = y2 - y1;
- b1 = x1 - x2;
- c1 = x2 * y1 - x1 * y2;
-
- /* Compute r3 and r4. */
-
- r3 = a1 * x3 + b1 * y3 + c1;
- r4 = a1 * x4 + b1 * y4 + c1;
-
- /* Check signs of r3 and r4. If both point 3 and point 4 lie on
- * same side of line 1, the line segments do not intersect. */
-
- if ( r3 != 0 &&
- r4 != 0 &&
- SAME_SIGNS( r3, r4 ))
- return ( DONT_INTERSECT );
-
- /* Compute a2, b2, c2 */
-
- a2 = y4 - y3;
- b2 = x3 - x4;
- c2 = x4 * y3 - x3 * y4;
-
- /* Compute r1 and r2 */
-
- r1 = a2 * x1 + b2 * y1 + c2;
- r2 = a2 * x2 + b2 * y2 + c2;
-
- /* Check signs of r1 and r2. If both point 1 and point 2 lie
- * on same side of second line segment, the line segments do
- * not intersect. */
-
- if ( r1 != 0 &&
- r2 != 0 &&
- SAME_SIGNS( r1, r2 ))
- return ( DONT_INTERSECT );
-
- /* Line segments intersect: compute intersection point. */
-
- denom = a1 * b2 - a2 * b1;
- if ( denom == 0 )
- return ( DONT_INTERSECT );
- offset = denom < 0 ? - denom / 2 : denom / 2;
-
- /* The denom/2 is to get rounding instead of truncating. It
- * is added or subtracted to the numerator, depending upon the
- * sign of the numerator. */
-
- num = b1 * c2 - b2 * c1;
- ix = ( num < 0 ? num - offset : num + offset ) / denom;
-
- num = a2 * c1 - a1 * c2;
- iy = ( num < 0 ? num - offset : num + offset ) / denom;
-
- return ( DO_INTERSECT );
-}
-
+/* Starshatter OpenSource Distribution
+ Copyright (c) 1997-2004, Destroyer Studios LLC.
+ All Rights Reserved.
+
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice,
+ this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+ * Neither the name "Destroyer Studios" nor the names of its contributors
+ may be used to endorse or promote products derived from this software
+ without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ POSSIBILITY OF SUCH DAMAGE.
+
+ SUBSYSTEM: nGenEx.lib
+ FILE: Geometry.cpp
+ AUTHOR: John DiCamillo
+
+
+ OVERVIEW
+ ========
+ Geometric Utilities
+*/
+
+#include "MemDebug.h"
+#include "Geometry.h"
+
+// +--------------------------------------------------------------------+
+
+void Rect::Inflate(int dx, int dy)
+{
+ x -= dx;
+ w += dx*2;
+ y -= dy;
+ h += dy*2;
+}
+
+void Rect::Deflate(int dx, int dy)
+{
+ x += dx;
+ w -= dx*2;
+ y += dy;
+ h -= dy*2;
+}
+
+void Rect::Inset(int l, int r, int t, int b)
+{
+ x += l;
+ y += t;
+ w -= l + r;
+ h -= t + b;
+}
+
+int Rect::Contains(int ax, int ay) const
+{
+ if (ax < x) return 0;
+ if (ax > x+w) return 0;
+ if (ay < y) return 0;
+ if (ay > y+h) return 0;
+
+ return 1;
+}
+
+// +--------------------------------------------------------------------+
+
+double
+Point::Normalize()
+{
+ double scale = 1.0;
+ double len = length();
+
+ if (len)
+ scale /= len;
+
+ x *= scale;
+ y *= scale;
+ z *= scale;
+
+ return len;
+}
+
+// +--------------------------------------------------------------------+
+
+void
+Point::SetElement(int i, double v)
+{
+ switch (i) {
+ case 0: x = v; break;
+ case 1: y = v; break;
+ case 2: z = v; break;
+ default: break;
+ }
+}
+
+// +--------------------------------------------------------------------+
+
+Point
+Point::operator*(const Matrix& m) const
+{
+ Point result;
+
+ result.x = (m.elem[0][0] * x) + (m.elem[1][0] * y) + (m.elem[2][0] * z);
+ result.y = (m.elem[0][1] * x) + (m.elem[1][1] * y) + (m.elem[2][1] * z);
+ result.z = (m.elem[0][2] * x) + (m.elem[1][2] * y) + (m.elem[2][2] * z);
+
+ return result;
+}
+
+// +--------------------------------------------------------------------+
+
+double ClosestApproachTime(const Point& loc1, const Point& vel1,
+const Point& loc2, const Point& vel2)
+{
+ double t = 0;
+
+ Point D = loc1-loc2;
+ Point Dv = vel1-vel2;
+
+ if (Dv.x || Dv.y || Dv.z)
+ t = -1 * (Dv*D) / (Dv*Dv);
+
+ return t;
+}
+
+// +--------------------------------------------------------------------+
+
+float
+Vec2::Normalize()
+{
+ float scale = 1.0f;
+ float len = length();
+
+ if (len)
+ scale /= len;
+
+ x *= scale;
+ y *= scale;
+
+ return len;
+}
+
+// +--------------------------------------------------------------------+
+
+float
+Vec3::Normalize()
+{
+ float scale = 1.0f;
+ float len = length();
+
+ if (len)
+ scale /= len;
+
+ x *= scale;
+ y *= scale;
+ z *= scale;
+
+ return len;
+}
+
+// +--------------------------------------------------------------------+
+
+Vec3
+Vec3::operator*(const Matrix& m) const
+{
+ Vec3 result;
+
+ result.x = (float) ((m.elem[0][0] * x) + (m.elem[1][0] * y) + (m.elem[2][0] * z));
+ result.y = (float) ((m.elem[0][1] * x) + (m.elem[1][1] * y) + (m.elem[2][1] * z));
+ result.z = (float) ((m.elem[0][2] * x) + (m.elem[1][2] * y) + (m.elem[2][2] * z));
+
+ return result;
+}
+
+// +--------------------------------------------------------------------+
+
+double ClosestApproachTime(const Vec3& loc1, const Vec3& vel1,
+const Vec3& loc2, const Vec3& vel2)
+{
+ double t = 0;
+
+ Point D = loc1-loc2;
+ Point Dv = vel1-vel2;
+
+ if (Dv.x || Dv.y || Dv.z)
+ t = -1 * (Dv*D) / (Dv*Dv);
+
+ return t;
+}
+
+// +--------------------------------------------------------------------+
+
+double
+Quaternion::Normalize()
+{
+ double scale = 1.0;
+ double len = length();
+
+ if (len)
+ scale /= len;
+
+ x *= scale;
+ y *= scale;
+ z *= scale;
+ w *= scale;
+
+ return len;
+}
+
+// +--------------------------------------------------------------------+
+
+Matrix::Matrix()
+{
+ Identity();
+}
+
+Matrix::Matrix(const Matrix& m)
+{
+ CopyMemory(elem, m.elem, sizeof(elem));
+}
+
+Matrix::Matrix(const Point& vrt, const Point& vup, const Point& vpn)
+{
+ elem[0][0] = vrt.x;
+ elem[0][1] = vrt.y;
+ elem[0][2] = vrt.z;
+
+ elem[1][0] = vup.x;
+ elem[1][1] = vup.y;
+ elem[1][2] = vup.z;
+
+ elem[2][0] = vpn.x;
+ elem[2][1] = vpn.y;
+ elem[2][2] = vpn.z;
+}
+
+// +--------------------------------------------------------------------+
+
+Matrix&
+Matrix::operator =(const Matrix& m)
+{
+ CopyMemory(elem, m.elem, sizeof(elem));
+
+ return *this;
+}
+
+// +--------------------------------------------------------------------+
+
+Matrix&
+Matrix::operator*=(const Matrix& m)
+{
+ return *this = *this * m;
+}
+
+// +--------------------------------------------------------------------+
+
+void
+Matrix::Identity()
+{
+ elem[0][0] = 1;
+ elem[0][1] = 0;
+ elem[0][2] = 0;
+
+ elem[1][0] = 0;
+ elem[1][1] = 1;
+ elem[1][2] = 0;
+
+ elem[2][0] = 0;
+ elem[2][1] = 0;
+ elem[2][2] = 1;
+}
+
+// +--------------------------------------------------------------------+
+
+inline void swap_elem(double& a, double& b) { double t=a; a=b; b=t; }
+
+void
+Matrix::Transpose()
+{
+ swap_elem(elem[0][1], elem[1][0]);
+ swap_elem(elem[0][2], elem[2][0]);
+ swap_elem(elem[1][2], elem[2][1]);
+}
+
+// +--------------------------------------------------------------------+
+
+void
+Matrix::Rotate(double roll, double pitch, double yaw)
+{
+ double e[3][3];
+ CopyMemory(e, elem, sizeof(elem));
+
+ double sr = sin(roll);
+ double cr = cos(roll);
+ double sp = sin(pitch);
+ double cp = cos(pitch);
+ double sy = sin(yaw);
+ double cy = cos(yaw);
+
+ double a,b,c;
+
+ a = cy*cr;
+ b = cy*sr;
+ c = -sy;
+
+ elem[0][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
+ elem[0][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
+ elem[0][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];
+
+ a = cp*-sr + sp*sy*cr;
+ b = cp* cr + sp*sy*sr;
+ c = sp*cy;
+
+ elem[1][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
+ elem[1][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
+ elem[1][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];
+
+ a = -sp*-sr + cp*sy*cr;
+ b = -sp* cr + cp*sy*sr;
+ c = cp*cy;
+
+ elem[2][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
+ elem[2][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
+ elem[2][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];
+}
+
+// +--------------------------------------------------------------------+
+
+void
+Matrix::Roll(double roll)
+{
+ double s = sin(roll);
+ double c = cos(roll);
+
+ double e00 = elem[0][0];
+ double e01 = elem[0][1];
+ double e02 = elem[0][2];
+ double e10 = elem[1][0];
+ double e11 = elem[1][1];
+ double e12 = elem[1][2];
+
+ elem[0][0] = c*e00 + s*e10;
+ elem[0][1] = c*e01 + s*e11;
+ elem[0][2] = c*e02 + s*e12;
+
+ elem[1][0] = -s*e00 + c*e10;
+ elem[1][1] = -s*e01 + c*e11;
+ elem[1][2] = -s*e02 + c*e12;
+}
+
+// +--------------------------------------------------------------------+
+
+void
+Matrix::Pitch(double pitch)
+{
+ double s = sin(pitch);
+ double c = cos(pitch);
+
+ double e10 = elem[1][0];
+ double e11 = elem[1][1];
+ double e12 = elem[1][2];
+ double e20 = elem[2][0];
+ double e21 = elem[2][1];
+ double e22 = elem[2][2];
+
+ elem[1][0] = c*e10 + s*e20;
+ elem[1][1] = c*e11 + s*e21;
+ elem[1][2] = c*e12 + s*e22;
+
+ elem[2][0] = -s*e10 + c*e20;
+ elem[2][1] = -s*e11 + c*e21;
+ elem[2][2] = -s*e12 + c*e22;
+}
+
+// +--------------------------------------------------------------------+
+
+void
+Matrix::Yaw(double yaw)
+{
+ double s = sin(yaw);
+ double c = cos(yaw);
+
+ double e00 = elem[0][0];
+ double e01 = elem[0][1];
+ double e02 = elem[0][2];
+ double e20 = elem[2][0];
+ double e21 = elem[2][1];
+ double e22 = elem[2][2];
+
+ elem[0][0] = c*e00 - s*e20;
+ elem[0][1] = c*e01 - s*e21;
+ elem[0][2] = c*e02 - s*e22;
+
+ elem[2][0] = s*e00 + c*e20;
+ elem[2][1] = s*e01 + c*e21;
+ elem[2][2] = s*e02 + c*e22;
+}
+
+// +--------------------------------------------------------------------+
+
+inline int sign(double d) { return (d >= 0); }
+
+void
+Matrix::ComputeEulerAngles(double& roll, double& pitch, double& yaw) const
+{
+ double cy;
+
+ yaw = asin(-elem[0][2]);
+ cy = cos(yaw);
+ roll = asin(elem[0][1] / cy);
+ pitch = asin(elem[1][2] / cy);
+
+ if (sign(cos(roll)*cy) != sign(elem[0][0]))
+ roll = PI - roll;
+
+ if (sign(cos(pitch)*cy) != sign(elem[2][2]))
+ pitch = PI - pitch;
+}
+
+// +--------------------------------------------------------------------+
+
+Matrix
+Matrix::operator*(const Matrix& m) const
+{
+ Matrix r;
+
+ r.elem[0][0] = elem[0][0]*m.elem[0][0] + elem[0][1]*m.elem[1][0] + elem[0][2]*m.elem[2][0];
+ r.elem[0][1] = elem[0][0]*m.elem[0][1] + elem[0][1]*m.elem[1][1] + elem[0][2]*m.elem[2][1];
+ r.elem[0][2] = elem[0][0]*m.elem[0][2] + elem[0][1]*m.elem[1][2] + elem[0][2]*m.elem[2][2];
+
+ r.elem[1][0] = elem[1][0]*m.elem[0][0] + elem[1][1]*m.elem[1][0] + elem[1][2]*m.elem[2][0];
+ r.elem[1][1] = elem[1][0]*m.elem[0][1] + elem[1][1]*m.elem[1][1] + elem[1][2]*m.elem[2][1];
+ r.elem[1][2] = elem[1][0]*m.elem[0][2] + elem[1][1]*m.elem[1][2] + elem[1][2]*m.elem[2][2];
+
+ r.elem[2][0] = elem[2][0]*m.elem[0][0] + elem[2][1]*m.elem[1][0] + elem[2][2]*m.elem[2][0];
+ r.elem[2][1] = elem[2][0]*m.elem[0][1] + elem[2][1]*m.elem[1][1] + elem[2][2]*m.elem[2][1];
+ r.elem[2][2] = elem[2][0]*m.elem[0][2] + elem[2][1]*m.elem[1][2] + elem[2][2]*m.elem[2][2];
+
+ return r;
+}
+
+// +--------------------------------------------------------------------+
+
+Point
+Matrix::operator*(const Point& p) const
+{
+ Point result;
+
+ result.x = (elem[0][0] * p.x) + (elem[0][1] * p.y) + (elem[0][2] * p.z);
+ result.y = (elem[1][0] * p.x) + (elem[1][1] * p.y) + (elem[1][2] * p.z);
+ result.z = (elem[2][0] * p.x) + (elem[2][1] * p.y) + (elem[2][2] * p.z);
+
+ return result;
+}
+
+// +--------------------------------------------------------------------+
+
+Vec3
+Matrix::operator*(const Vec3& v) const
+{
+ Vec3 result;
+
+ result.x = (float) ((elem[0][0] * v.x) + (elem[0][1] * v.y) + (elem[0][2] * v.z));
+ result.y = (float) ((elem[1][0] * v.x) + (elem[1][1] * v.y) + (elem[1][2] * v.z));
+ result.z = (float) ((elem[2][0] * v.x) + (elem[2][1] * v.y) + (elem[2][2] * v.z));
+
+ return result;
+}
+
+// +--------------------------------------------------------------------+
+
+double
+Matrix::Cofactor(int i, int j) const
+{
+ int i1=0;
+ int i2=2;
+ int j1=0;
+ int j2=2;
+
+ if (i==0) i1=1; else if (i==2) i2=1;
+ if (j==0) j1=1; else if (j==2) j2=1;
+
+ double factor = elem[i1][j1]*elem[i2][j2] - elem[i1][j2]*elem[i2][j1];
+
+ if ((i+j) & 1)
+ factor *= -1;
+
+ return factor;
+}
+
+// +--------------------------------------------------------------------+
+
+void
+Matrix::Invert()
+{
+ double f[3][3];
+ int i, j;
+
+ for (i = 0; i < 3; i++)
+ for (j = 0; j < 3; j++)
+ f[i][j] = Cofactor(j,i);
+
+ double det = elem[0][0] * f[0][0] +
+ elem[0][1] * f[1][0] +
+ elem[0][2] * f[2][0];
+
+ if (det != 0) {
+ double inv = 1/det;
+
+ for (i = 0; i < 3; i++)
+ for (j = 0; j < 3; j++)
+ elem[i][j] = f[i][j] * inv;
+ }
+}
+
+// +--------------------------------------------------------------------+
+// +--------------------------------------------------------------------+
+// +--------------------------------------------------------------------+
+
+Plane::Plane()
+: distance(0.0f)
+{ }
+
+Plane::Plane(const Point& p0, const Point& p1, const Point& p2)
+{
+ Point d1 = p1 - p0;
+ Point d2 = p2 - p0;
+
+ normal = (Vec3) d1.cross(d2);
+ normal.Normalize();
+
+ distance = (float) (normal * p0);
+}
+
+Plane::Plane(const Vec3& v0, const Vec3& v1, const Vec3& v2)
+{
+ Vec3 d1 = v1 - v0;
+ Vec3 d2 = v2 - v0;
+
+ normal = d1.cross(d2);
+ normal.Normalize();
+
+ distance = normal * v0;
+}
+
+void Plane::Rotate(const Vec3& v0, const Matrix& m)
+{
+ normal = normal * m;
+ distance = normal * v0;
+}
+
+void Plane::Translate(const Vec3& v0)
+{
+ distance = normal * v0;
+}
+
+// +--------------------------------------------------------------------+
+// 3-D dot product.
+
+double DotProduct(const Point& a, const Point& b)
+{
+ return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
+}
+
+// +--------------------------------------------------------------------+
+// 3-D cross product.
+
+void CrossProduct(const Point& a, const Point& b, Point& out)
+{
+ out.x = (a.y * b.z) - (a.z * b.y);
+ out.y = (a.z * b.x) - (a.x * b.z);
+ out.z = (a.x * b.y) - (a.y * b.x);
+}
+
+// +--------------------------------------------------------------------+
+// Concatenate two 3x3 matrices.
+
+void MConcat(double in1[3][3], double in2[3][3], double out[3][3])
+{
+ int i, j;
+
+ for (i=0 ; i<3 ; i++) {
+ for (j=0 ; j<3 ; j++) {
+ out[i][j] = in1[i][0] * in2[0][j] +
+ in1[i][1] * in2[1][j] +
+ in1[i][2] * in2[2][j];
+ }
+ }
+}
+
+/* GRAPHICS GEMS II ----------------------------------------------------
+*
+* lines_intersect: AUTHOR: Mukesh Prasad
+*
+* This function computes whether two line segments,
+* respectively joining the input points (x1,y1) -- (x2,y2)
+* and the input points (x3,y3) -- (x4,y4) intersect.
+* If the lines intersect, the output variables x, y are
+* set to coordinates of the point of intersection.
+*
+* All values are in integers. The returned value is rounded
+* to the nearest integer point.
+*
+* If non-integral grid points are relevant, the function
+* can easily be transformed by substituting floating point
+* calculations instead of integer calculations.
+*
+* Entry
+* x1, y1, x2, y2 Coordinates of endpoints of one segment.
+* x3, y3, x4, y4 Coordinates of endpoints of other segment.
+*
+* Exit
+* x, y Coordinates of intersection point.
+*
+* The value returned by the function is one of:
+*
+* DONT_INTERSECT 0
+* DO_INTERSECT 1
+* COLLINEAR 2
+*
+* Error conditions:
+*
+* Depending upon the possible ranges, and particularly on 16-bit
+* computers, care should be taken to protect from overflow.
+*
+* In the following code, 'long' values have been used for this
+* purpose, instead of 'int'.
+*
+*/
+
+#define DONT_INTERSECT 0
+#define DO_INTERSECT 1
+#define COLLINEAR 2
+
+inline int SAME_SIGNS(double a, double b)
+{
+ return ((a>=0 && b>=0) || (a<0 && b<0));
+}
+
+int
+lines_intersect(
+/* 1st line segment */ double x1, double y1, double x2, double y2,
+/* 2nd line segment */ double x3, double y3, double x4, double y4,
+/* pt of intersect */ double& ix, double& iy)
+{
+ double a1, a2, b1, b2, c1, c2; /* Coefficients of line eqns. */
+ double r1, r2, r3, r4; /* 'Sign' values */
+ double denom, offset, num; /* Intermediate values */
+
+ /* Compute a1, b1, c1, where line joining points 1 and 2
+ * is "a1 x + b1 y + c1 = 0". */
+
+ a1 = y2 - y1;
+ b1 = x1 - x2;
+ c1 = x2 * y1 - x1 * y2;
+
+ /* Compute r3 and r4. */
+
+ r3 = a1 * x3 + b1 * y3 + c1;
+ r4 = a1 * x4 + b1 * y4 + c1;
+
+ /* Check signs of r3 and r4. If both point 3 and point 4 lie on
+ * same side of line 1, the line segments do not intersect. */
+
+ if ( r3 != 0 &&
+ r4 != 0 &&
+ SAME_SIGNS( r3, r4 ))
+ return ( DONT_INTERSECT );
+
+ /* Compute a2, b2, c2 */
+
+ a2 = y4 - y3;
+ b2 = x3 - x4;
+ c2 = x4 * y3 - x3 * y4;
+
+ /* Compute r1 and r2 */
+
+ r1 = a2 * x1 + b2 * y1 + c2;
+ r2 = a2 * x2 + b2 * y2 + c2;
+
+ /* Check signs of r1 and r2. If both point 1 and point 2 lie
+ * on same side of second line segment, the line segments do
+ * not intersect. */
+
+ if ( r1 != 0 &&
+ r2 != 0 &&
+ SAME_SIGNS( r1, r2 ))
+ return ( DONT_INTERSECT );
+
+ /* Line segments intersect: compute intersection point. */
+
+ denom = a1 * b2 - a2 * b1;
+ if ( denom == 0 )
+ return ( DONT_INTERSECT );
+ offset = denom < 0 ? - denom / 2 : denom / 2;
+
+ /* The denom/2 is to get rounding instead of truncating. It
+ * is added or subtracted to the numerator, depending upon the
+ * sign of the numerator. */
+
+ num = b1 * c2 - b2 * c1;
+ ix = ( num < 0 ? num - offset : num + offset ) / denom;
+
+ num = a2 * c1 - a1 * c2;
+ iy = ( num < 0 ? num - offset : num + offset ) / denom;
+
+ return ( DO_INTERSECT );
+}
+
generated by cgit v1.1 at 2024-11-04 21:05:33 +0000