1 #include "V3.h"
2 #include "R3Matrix.h"
3 #include "Plane.h"
4
normalize()5 void V3::normalize() {
6 PFloat d;
7
8 d = 1 / magnitude();
9 mX *= d;
10 mY *= d;
11 mZ *= d;
12 }
13
14
15
16
transform(const R3Matrix & inMatrix)17 void V3::transform( const R3Matrix& inMatrix ) {
18 PFloat y = mY, x = mX;
19
20 mX = inMatrix.mM[0] * x + inMatrix.mM[1] * y + inMatrix.mM[2] * mZ;
21 mY = inMatrix.mM[3] * x + inMatrix.mM[4] * y + inMatrix.mM[5] * mZ;
22 mZ = inMatrix.mM[6] * x + inMatrix.mM[7] * y + inMatrix.mM[8] * mZ;
23 }
24
25
26
transform(const R3Matrix & inMatrix,float inPerspectiveZ)27 void V3::transform( const R3Matrix& inMatrix, float inPerspectiveZ ) {
28 PFloat y = mY, x = mX;
29 PFloat xt, yt;
30
31 xt = inMatrix.mM[0] * x + inMatrix.mM[1] * y + inMatrix.mM[2] * mZ;
32 yt = inMatrix.mM[3] * x + inMatrix.mM[4] * y + inMatrix.mM[5] * mZ;
33 mZ = inMatrix.mM[6] * x + inMatrix.mM[7] * y + inMatrix.mM[8] * mZ;
34
35 // Catch float div by zeros...
36 x = mZ + inPerspectiveZ;
37 mX = xt / x;
38 mY = yt / x;
39 }
40
41
42
43
transform(const R3Matrix & inMatrix,const V3 & inPt)44 void V3::transform( const R3Matrix& inMatrix, const V3& inPt ) {
45 PFloat y = inPt.mY, x = inPt.mX, z = inPt.mZ;
46
47 mX = inMatrix.mM[0] * x + inMatrix.mM[1] * y + inMatrix.mM[2] * z;
48 mY = inMatrix.mM[3] * x + inMatrix.mM[4] * y + inMatrix.mM[5] * z;
49 mZ = inMatrix.mM[6] * x + inMatrix.mM[7] * y + inMatrix.mM[8] * z;
50 }
51
52
53
cross(const V3 & inV)54 void V3::cross( const V3& inV ) {
55 PFloat x = mX, y = mY;
56
57 mX = inV.mZ * y - mZ * inV.mY;
58 mY = mZ * inV.mX - inV.mZ * x;
59 mZ = x * inV.mY - inV.mX * y;
60 }
61
62
63
64
cross(const V3 & inA,const V3 & inB)65 void V3::cross( const V3& inA, const V3& inB ) {
66
67 mX = inA.mZ * inB.mY - inB.mZ * inA.mY;
68 mY = inB.mZ * inA.mX - inA.mZ * inB.mX;
69 mZ = inB.mX * inA.mY - inA.mX * inB.mY;
70 }
71
72
rotate(const V3 & inPt1,const V3 & inPt2,PFloat inAng)73 void V3::rotate( const V3& inPt1, const V3& inPt2, PFloat inAng ) {
74 PFloat x, s, c;
75 V3 line;
76
77 line.set( inPt1 );
78 line.subtract( inPt2 );
79 subtract( inPt1 );
80 toPlane( line );
81 s = sin( inAng );
82 c = cos( inAng );
83 x = mX;
84 mX = x * c - mY * s;
85 mY = x * s + mY * c;
86 fromPlane( line );
87 add( inPt1 );
88 }
89
90 #define TOO_BIG 1.0e20
91
92
intersection(const Plane & inPlane,const V3 & inLine,const V3 & inPt)93 bool V3::intersection( const Plane& inPlane, const V3& inLine, const V3& inPt ) {
94
95 PFloat t = ( inPlane.mD - inPlane.dot( inPt ) ) / inPlane.dot( inLine );
96 set( inLine );
97 scale( t );
98 add( inPt );
99
100 return t > - TOO_BIG && t < TOO_BIG;
101 }
102
103
104
105
106
107 #define A inNormal.mX
108 #define B inNormal.mY
109 #define C inNormal.mZ
110
toPlane(const V3 & inNormal)111 void V3::toPlane( const V3& inNormal ) {
112 PFloat BC = sqrt( B*B + C*C );
113 PFloat ABC = inNormal.magnitude();
114 PFloat x = mX, y = mY;
115
116 if ( BC > 0.0001 ) {
117 mX = x * BC / ABC - A * ( B*y + C*mZ ) / (ABC * BC);
118 mY = (C*y - B*mZ) / BC;
119 mZ = (A*x + B*y + C*mZ) / ABC; }
120 else {
121 mX = mZ;
122 mZ = - x;
123 }
124
125 }
126
127
128
129
fromPlane(const V3 & inNormal)130 void V3::fromPlane( const V3& inNormal ) {
131 PFloat BC = sqrt( B*B + C*C );
132 PFloat ABC = inNormal.magnitude();
133 PFloat x = mX, y = mY;
134
135 if ( BC > 0.0001 ) {
136 mX = (x * BC + A * mZ ) / ABC;
137 mY = C * y / BC - A * B * x / ( BC*ABC ) + B * mZ / ABC;
138 mZ = - B * y / BC - A * C * x / ( BC*ABC ) + C * mZ / ABC; }
139 else {
140 mX = - mZ;
141 mZ = x;
142 }
143 }
144
145