00001 #include "Geometry.h"
00002
00003 Point Geometry::PlaneIntersection (const Plane& p1, const Plane& p2, const Plane& p3)
00004 {
00005 Point result;
00006 float denominator;
00007 Vector t1, t2, t3;
00008
00009
00010 denominator = ScalarProduct (p1.n, CrossProduct (p2.n, p3.n));
00011
00012 if (denominator == 0.0f)
00013 {
00014 result.x = 0.0f;
00015 result.y = 0.0f;
00016 result.z = 0.0f;
00017 }
00018 else
00019 {
00020 t1 = CrossProduct (p2.n, p3.n);
00021 t2 = CrossProduct (p3.n, p1.n);
00022 t3 = CrossProduct (p1.n, p2.n);
00023
00024 result.x = (- t1.x * p1.d - t2.x * p2.d - t3.x * p3.d) / denominator;
00025 result.y = (- t1.y * p1.d - t2.y * p2.d - t3.y * p3.d) / denominator;
00026 result.z = (- t1.z * p1.d - t2.z * p2.d - t3.z * p3.d) / denominator;
00027 }
00028
00029 return result;
00030 }
00031
00032 Vector Geometry::CrossProduct (const Vector& v1, const Vector& v2)
00033 {
00034 Vector result;
00035
00036 result.x = v1.y * v2.z - v2.y * v1.z;
00037 result.y = v1.z * v2.x - v2.z * v1.x;
00038 result.z = v1.x * v2.y - v2.x * v1.y;
00039
00040 return result;
00041 }
00042
00043 float Geometry::ScalarProduct (const Vector& v1, const Vector& v2)
00044 {
00045 return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
00046 }
00047
00048 void Geometry::InvertHomogenousMatrix (double * src, double * dest)
00049 {
00050 dest[0] = src[0];
00051 dest[1] = src[4];
00052 dest[2] = src[8];
00053 dest[3] = 0.0;
00054
00055 dest[4] = src[1];
00056 dest[5] = src[5];
00057 dest[6] = src[9];
00058 dest[7] = 0.0;
00059
00060 dest[8] = src[2];
00061 dest[9] = src[6];
00062 dest[10]= src[10];
00063 dest[11]= 0.0;
00064
00065 dest[12] = - (dest[0] * src[12] + dest[4] * src[13] + dest[8] * src[14]);
00066 dest[13] = - (dest[1] * src[12] + dest[5] * src[13] + dest[9] * src[14]);
00067 dest[14] = - (dest[2] * src[12] + dest[6] * src[13] + dest[10] * src[14]);
00068 dest[15] = 1.0;
00069 }
00070
00071 void Geometry::InvertHomogenousMatrix (float * src, float * dest)
00072 {
00073 dest[0] = src[0];
00074 dest[1] = src[4];
00075 dest[2] = src[8];
00076 dest[3] = 0.0;
00077
00078 dest[4] = src[1];
00079 dest[5] = src[5];
00080 dest[6] = src[9];
00081 dest[7] = 0.0;
00082
00083 dest[8] = src[2];
00084 dest[9] = src[6];
00085 dest[10]= src[10];
00086 dest[11]= 0.0;
00087
00088 dest[12] = - (dest[0] * src[12] + dest[4] * src[13] + dest[8] * src[14]);
00089 dest[13] = - (dest[1] * src[12] + dest[5] * src[13] + dest[9] * src[14]);
00090 dest[14] = - (dest[2] * src[12] + dest[6] * src[13] + dest[10] * src[14]);
00091 dest[15] = 1.0;
00092 }
00093
00094 void Geometry::MatrixMultiply (double * m1, double * m2, double * dest)
00095 {
00096 dest[0] = m2[0] * m1[0] + m2[1] * m1[4] + m2[2] * m1[8] + m2[3] * m1[12];
00097 dest[1] = m2[0] * m1[1] + m2[1] * m1[5] + m2[2] * m1[9] + m2[3] * m1[13];
00098 dest[2] = m2[0] * m1[2] + m2[1] * m1[6] + m2[2] * m1[10] + m2[3] * m1[14];
00099 dest[3] = m2[0] * m1[3] + m2[1] * m1[7] + m2[2] * m1[11] + m2[3] * m1[15];
00100 dest[4] = m2[4] * m1[0] + m2[5] * m1[4] + m2[6] * m1[8] + m2[7] * m1[12];
00101 dest[5] = m2[4] * m1[1] + m2[5] * m1[5] + m2[6] * m1[9] + m2[7] * m1[13];
00102 dest[6] = m2[4] * m1[2] + m2[5] * m1[6] + m2[6] * m1[10] + m2[7] * m1[14];
00103 dest[7] = m2[4] * m1[3] + m2[5] * m1[7] + m2[6] * m1[11] + m2[7] * m1[15];
00104 dest[8] = m2[8] * m1[0] + m2[9] * m1[4] + m2[10] * m1[8] + m2[11] * m1[12];
00105 dest[9] = m2[8] * m1[1] + m2[9] * m1[5] + m2[10] * m1[9] + m2[11] * m1[13];
00106 dest[10] = m2[8] * m1[2] + m2[9] * m1[6] + m2[10] * m1[10] + m2[11] * m1[14];
00107 dest[11] = m2[8] * m1[3] + m2[9] * m1[7] + m2[10] * m1[11] + m2[11] * m1[15];
00108 dest[12] = m2[12] * m1[0] + m2[13] * m1[4] + m2[14] * m1[8] + m2[15] * m1[12];
00109 dest[13] = m2[12] * m1[1] + m2[13] * m1[5] + m2[14] * m1[9] + m2[15] * m1[13];
00110 dest[14] = m2[12] * m1[2] + m2[13] * m1[6] + m2[14] * m1[10] + m2[15] * m1[14];
00111 dest[15] = m2[12] * m1[3] + m2[13] * m1[7] + m2[14] * m1[11] + m2[15] * m1[15];
00112 }
00113
00114 void Geometry::VectorMultiply (float * m, const Vector & v, Vector & r)
00115 {
00116 r.x = m[0] * v.x + m[4] * v.y + m[8] * v.z + m[12];
00117 r.y = m[1] * v.x + m[5] * v.y + m[9] * v.z + m[13];
00118 r.z = m[2] * v.x + m[6] * v.y + m[10] * v.z + m[14];
00119 }
00120
00121 void Geometry::VectorNormalize (Vector & vect)
00122 {
00123 float length;
00124
00125 length = sqrt (vect.x * vect.x + vect.y * vect.y + vect.z * vect.z);
00126 if (length > 0)
00127 {
00128 vect.x /= length;
00129 vect.y /= length;
00130 vect.z /= length;
00131 }
00132 }
00133
00134 float Geometry::VectorLength (Vector & vect)
00135 {
00136 return sqrt (vect.x * vect.x + vect.y * vect.y + vect.z * vect.z);
00137 }
00138
00139 float Geometry::EnclosingAngle (Vector & v1, Vector & v2)
00140 {
00141 float cos_of_angle;
00142 float angle;
00143
00144 cos_of_angle = ScalarProduct (v1, v2) / (VectorLength (v1) * VectorLength (v2));
00145 angle = acos (cos_of_angle);
00146
00147 return angle;
00148 }
00149
