9 void Add(Vector3D vector1, Vector3D vector2)
11 x = vector1.x + vector2.x;
12 y = vector1.y + vector2.y;
13 z = vector1.z + vector2.z;
16 void Subtract(Vector3D vector1, Vector3D vector2)
18 x = vector1.x - vector2.x;
19 y = vector1.y - vector2.y;
20 z = vector1.z - vector2.z;
23 void Scale(Vector3D vector1, double s)
30 double DotProduct(Vector3D vector2)
32 return x * vector2.x + y * vector2.y + z * vector2.z;
35 double DotProductf(Vector3Df vector2)
37 return x * vector2.x + y * vector2.y + z * vector2.z;
40 void CrossProduct(Vector3D vector1, Vector3D vector2)
42 x = vector1.y * vector2.z - vector1.z * vector2.y;
43 y = vector1.z * vector2.x - vector1.x * vector2.z;
44 z = vector1.x * vector2.y - vector1.y * vector2.x;
47 void Normalize(Vector3D source)
49 double m = source.length;
60 void MultMatrix(Vector3D source, Matrix matrix)
62 x = source.x * matrix.m[0][0] + source.y * matrix.m[1][0] + source.z * matrix.m[2][0] + matrix.m[3][0];
63 y = source.x * matrix.m[0][1] + source.y * matrix.m[1][1] + source.z * matrix.m[2][1] + matrix.m[3][1];
64 z = source.x * matrix.m[0][2] + source.y * matrix.m[1][2] + source.z * matrix.m[2][2] + matrix.m[3][2];
67 void MultMatrixf(Vector3Df source, Matrix matrix)
69 x = source.x * matrix.m[0][0] + source.y * matrix.m[1][0] + source.z * matrix.m[2][0] + matrix.m[3][0];
70 y = source.x * matrix.m[0][1] + source.y * matrix.m[1][1] + source.z * matrix.m[2][1] + matrix.m[3][1];
71 z = source.x * matrix.m[0][2] + source.y * matrix.m[1][2] + source.z * matrix.m[2][2] + matrix.m[3][2];
74 void DivideMatrix(Vector3D source, Matrix matrix)
79 vectorX=sourceX*m00+sourceY*m10+sourceZ*m20+m30,
80 vectorY=sourceZ*m01+sourceY*m11+sourceZ*m21+m31,
81 vectorZ=sourceX*m02+sourceY*m12+sourceZ*m22+m32
82 }, { sourceX, sourceY, sourceZ });
86 matrix.m[2][0] * matrix.m[0][2] * matrix.m[1][1]
87 - matrix.m[0][2] * matrix.m[2][1] * matrix.m[1][0]
88 - matrix.m[2][2] * matrix.m[0][0] * matrix.m[1][1]
89 - matrix.m[0][2] * matrix.m[0][1] * matrix.m[1][0]
90 + matrix.m[2][1] * matrix.m[0][0] * matrix.m[1][2]
91 + matrix.m[0][1] * matrix.m[0][0] * matrix.m[1][2];
94 - matrix.m[2][2] * source.x * matrix.m[1][1]
95 + matrix.m[2][2] * matrix.m[1][0] * source.y
96 - matrix.m[2][2] * matrix.m[1][0] * matrix.m[3][1]
97 + matrix.m[2][2] * matrix.m[3][0] * matrix.m[1][1]
98 - matrix.m[2][0] * matrix.m[3][2] * matrix.m[1][1]
99 + source.x * matrix.m[2][1] * matrix.m[1][2]
100 + source.x * matrix.m[0][1] * matrix.m[1][2]
101 - matrix.m[1][0] * matrix.m[0][1] * source.z
102 + matrix.m[1][0] * matrix.m[2][1] * matrix.m[3][2]
103 + matrix.m[1][0] * matrix.m[0][1] * matrix.m[3][2]
104 - matrix.m[1][0] * matrix.m[2][1] * source.z
105 - matrix.m[3][0] * matrix.m[2][1] * matrix.m[1][2]
106 - matrix.m[2][0] * matrix.m[1][2] * source.y
107 + matrix.m[2][0] * matrix.m[1][2] * matrix.m[3][1]
108 + matrix.m[2][0] * source.z * matrix.m[1][1]
109 - matrix.m[3][0] * matrix.m[0][1] * matrix.m[1][2]
113 - matrix.m[2][0] * matrix.m[0][2] * source.y
114 + matrix.m[2][1] * matrix.m[0][0] * matrix.m[3][2]
115 + matrix.m[2][0] * matrix.m[0][2] * matrix.m[3][1]
116 + matrix.m[0][1] * matrix.m[0][0] * matrix.m[3][2]
117 - matrix.m[2][1] * matrix.m[0][0] * source.z
118 - matrix.m[0][2] * matrix.m[2][1] * matrix.m[3][0]
119 + matrix.m[0][2] * matrix.m[0][1] * source.x
120 - matrix.m[0][2] * matrix.m[0][1] * matrix.m[3][0]
121 + matrix.m[0][2] * matrix.m[2][1] * source.x
122 + matrix.m[2][2] * matrix.m[0][0] * source.y
123 - matrix.m[0][1] * matrix.m[0][0] * source.z
124 - matrix.m[2][2] * matrix.m[0][0] * matrix.m[3][1]
128 source.x * matrix.m[0][2] * matrix.m[1][1]
129 + matrix.m[0][0] * matrix.m[3][2] * matrix.m[1][1]
130 + matrix.m[0][0] * matrix.m[1][2] * source.y
131 - matrix.m[0][0] * matrix.m[1][2] * matrix.m[3][1]
132 - matrix.m[0][0] * source.z * matrix.m[1][1]
133 - matrix.m[1][0] * matrix.m[0][2] * source.y
134 + matrix.m[1][0] * matrix.m[0][2] * matrix.m[3][1]
135 - matrix.m[3][0] * matrix.m[0][2] * matrix.m[1][1]
139 property double length { get { return (double)sqrt(x * x + y * y + z * z); } };
140 property double lengthApprox
144 double ix = Abs(x), iy = Abs(y), iz = Abs(z);
146 if(ix < iy) { tmp = ix; ix = iy; iy = tmp; }
147 if(ix < iz) { tmp = ix; ix = iz; iz = tmp; }
148 if(iz > iy) { iz = iy; }
154 public /*inline */float FastInvSqrt(float x)
156 union { float f; uint u; } i;
159 i.u = 0x5f375a86 - (i.u >> 1);
161 return x * (1.5f - (halfX * x * x));
164 public /*inline */double FastInvSqrtDouble(double x)
166 union { double d; uint64 u; } i;
167 double halfX = x / 2;
169 i.u = 0x5fe6eb50c7b537a9LL - (i.u >> 1);
171 return x * (1.5 - (halfX * x * x));
174 public struct Vector3Df
178 void Add(Vector3Df vector1, Vector3Df vector2)
180 x = vector1.x + vector2.x;
181 y = vector1.y + vector2.y;
182 z = vector1.z + vector2.z;
185 void Subtract(Vector3Df vector1, Vector3Df vector2)
187 x = vector1.x - vector2.x;
188 y = vector1.y - vector2.y;
189 z = vector1.z - vector2.z;
192 void Scale(Vector3Df vector1, float s)
199 double DotProduct(Vector3Df vector2)
201 return (double)x * (double)vector2.x + (double)y * (double)vector2.y + (double)z * (double)vector2.z;
204 void CrossProduct(Vector3Df vector1, Vector3Df vector2)
206 x = vector1.y * vector2.z - vector1.z * vector2.y;
207 y = vector1.z * vector2.x - vector1.x * vector2.z;
208 z = vector1.x * vector2.y - vector1.y * vector2.x;
211 void Normalize(Vector3Df source)
214 float m = source.length;
224 float i = FastInvSqrt(source.x * source.x + source.y * source.y + source.z * source.z);
230 void MultMatrix(Vector3Df source, Matrix matrix)
232 x = (float)(source.x * matrix.m[0][0] + source.y * matrix.m[1][0] + source.z * matrix.m[2][0] + matrix.m[3][0]);
233 y = (float)(source.x * matrix.m[0][1] + source.y * matrix.m[1][1] + source.z * matrix.m[2][1] + matrix.m[3][1]);
234 z = (float)(source.x * matrix.m[0][2] + source.y * matrix.m[1][2] + source.z * matrix.m[2][2] + matrix.m[3][2]);
237 void DivideMatrix(Vector3Df source, Matrix matrix)
242 vectorX=sourceX*m00+sourceY*m10+sourceZ*m20+m30,
243 vectorY=sourceZ*m01+sourceY*m11+sourceZ*m21+m31,
244 vectorZ=sourceX*m02+sourceY*m12+sourceZ*m22+m32
245 }, { sourceX, sourceY, sourceZ });
248 float var1 = (float)(
249 matrix.m[2][0] * matrix.m[0][2] * matrix.m[1][1]
250 - matrix.m[0][2] * matrix.m[2][1] * matrix.m[1][0]
251 - matrix.m[2][2] * matrix.m[0][0] * matrix.m[1][1]
252 - matrix.m[0][2] * matrix.m[0][1] * matrix.m[1][0]
253 + matrix.m[2][1] * matrix.m[0][0] * matrix.m[1][2]
254 + matrix.m[0][1] * matrix.m[0][0] * matrix.m[1][2]);
257 - matrix.m[2][2] * source.x * matrix.m[1][1]
258 + matrix.m[2][2] * matrix.m[1][0] * source.y
259 - matrix.m[2][2] * matrix.m[1][0] * matrix.m[3][1]
260 + matrix.m[2][2] * matrix.m[3][0] * matrix.m[1][1]
261 - matrix.m[2][0] * matrix.m[3][2] * matrix.m[1][1]
262 + source.x * matrix.m[2][1] * matrix.m[1][2]
263 + source.x * matrix.m[0][1] * matrix.m[1][2]
264 - matrix.m[1][0] * matrix.m[0][1] * source.z
265 + matrix.m[1][0] * matrix.m[2][1] * matrix.m[3][2]
266 + matrix.m[1][0] * matrix.m[0][1] * matrix.m[3][2]
267 - matrix.m[1][0] * matrix.m[2][1] * source.z
268 - matrix.m[3][0] * matrix.m[2][1] * matrix.m[1][2]
269 - matrix.m[2][0] * matrix.m[1][2] * source.y
270 + matrix.m[2][0] * matrix.m[1][2] * matrix.m[3][1]
271 + matrix.m[2][0] * source.z * matrix.m[1][1]
272 - matrix.m[3][0] * matrix.m[0][1] * matrix.m[1][2]
276 - matrix.m[2][0] * matrix.m[0][2] * source.y
277 + matrix.m[2][1] * matrix.m[0][0] * matrix.m[3][2]
278 + matrix.m[2][0] * matrix.m[0][2] * matrix.m[3][1]
279 + matrix.m[0][1] * matrix.m[0][0] * matrix.m[3][2]
280 - matrix.m[2][1] * matrix.m[0][0] * source.z
281 - matrix.m[0][2] * matrix.m[2][1] * matrix.m[3][0]
282 + matrix.m[0][2] * matrix.m[0][1] * source.x
283 - matrix.m[0][2] * matrix.m[0][1] * matrix.m[3][0]
284 + matrix.m[0][2] * matrix.m[2][1] * source.x
285 + matrix.m[2][2] * matrix.m[0][0] * source.y
286 - matrix.m[0][1] * matrix.m[0][0] * source.z
287 - matrix.m[2][2] * matrix.m[0][0] * matrix.m[3][1]
291 source.x * matrix.m[0][2] * matrix.m[1][1]
292 + matrix.m[0][0] * matrix.m[3][2] * matrix.m[1][1]
293 + matrix.m[0][0] * matrix.m[1][2] * source.y
294 - matrix.m[0][0] * matrix.m[1][2] * matrix.m[3][1]
295 - matrix.m[0][0] * source.z * matrix.m[1][1]
296 - matrix.m[1][0] * matrix.m[0][2] * source.y
297 + matrix.m[1][0] * matrix.m[0][2] * matrix.m[3][1]
298 - matrix.m[3][0] * matrix.m[0][2] * matrix.m[1][1]
302 property float length { get { return (float)sqrt(x * x + y * y + z * z); } };
303 property float lengthApprox
307 float ix = Abs(x), iy = Abs(y), iz = Abs(z);
309 if(ix < iy) { tmp = ix; ix = iy; iy = tmp; }
310 if(ix < iz) { tmp = ix; ix = iz; iz = tmp; }
311 if(iz > iy) { iz = iy; }
320 value.x = (double) x;
321 value.y = (double) y;
322 value.z = (double) z;