1/*
2Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. This notice may not be removed or altered from any source distribution.
13*/
14
15#ifndef BT_VECTOR3_H
16#define BT_VECTOR3_H
17
18#include "btMinMax.h"
19#include "btScalar.h"
20
21#ifdef BT_USE_DOUBLE_PRECISION
22#define btVector3Data btVector3DoubleData
23#define btVector3DataName "btVector3DoubleData"
24#else
25#define btVector3Data btVector3FloatData
26#define btVector3DataName "btVector3FloatData"
27#endif //BT_USE_DOUBLE_PRECISION
28
29// -- GODOT start --
30namespace VHACD {
31// -- GODOT end --
32
33/**@brief btVector3 can be used to represent 3D points and vectors.
34 * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
35 * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
36 */
37ATTRIBUTE_ALIGNED16(class)
38btVector3
39{
40public:
41#if defined(__SPU__) && defined(__CELLOS_LV2__)
42 btScalar m_floats[4];
43
44public:
45 SIMD_FORCE_INLINE const vec_float4& get128() const
46 {
47 return *((const vec_float4*)&m_floats[0]);
48 }
49
50public:
51#else //__CELLOS_LV2__ __SPU__
52#ifdef BT_USE_SSE // _WIN32
53 union {
54 __m128 mVec128;
55 btScalar m_floats[4];
56 };
57 SIMD_FORCE_INLINE __m128 get128() const
58 {
59 return mVec128;
60 }
61 SIMD_FORCE_INLINE void set128(__m128 v128)
62 {
63 mVec128 = v128;
64 }
65#else
66 btScalar m_floats[4];
67#endif
68#endif //__CELLOS_LV2__ __SPU__
69
70public:
71 /**@brief No initialization constructor */
72 SIMD_FORCE_INLINE btVector3() {}
73
74 /**@brief Constructor from scalars
75 * @param x X value
76 * @param y Y value
77 * @param z Z value
78 */
79 SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
80 {
81 m_floats[0] = x;
82 m_floats[1] = y;
83 m_floats[2] = z;
84 m_floats[3] = btScalar(0.);
85 }
86
87 /**@brief Add a vector to this one
88 * @param The vector to add to this one */
89 SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
90 {
91
92 m_floats[0] += v.m_floats[0];
93 m_floats[1] += v.m_floats[1];
94 m_floats[2] += v.m_floats[2];
95 return *this;
96 }
97
98 /**@brief Subtract a vector from this one
99 * @param The vector to subtract */
100 SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
101 {
102 m_floats[0] -= v.m_floats[0];
103 m_floats[1] -= v.m_floats[1];
104 m_floats[2] -= v.m_floats[2];
105 return *this;
106 }
107 /**@brief Scale the vector
108 * @param s Scale factor */
109 SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
110 {
111 m_floats[0] *= s;
112 m_floats[1] *= s;
113 m_floats[2] *= s;
114 return *this;
115 }
116
117 /**@brief Inversely scale the vector
118 * @param s Scale factor to divide by */
119 SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s)
120 {
121 btFullAssert(s != btScalar(0.0));
122 return * this *= btScalar(1.0) / s;
123 }
124
125 /**@brief Return the dot product
126 * @param v The other vector in the dot product */
127 SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
128 {
129 return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] + m_floats[2] * v.m_floats[2];
130 }
131
132 /**@brief Return the length of the vector squared */
133 SIMD_FORCE_INLINE btScalar length2() const
134 {
135 return dot(*this);
136 }
137
138 /**@brief Return the length of the vector */
139 SIMD_FORCE_INLINE btScalar length() const
140 {
141 return btSqrt(length2());
142 }
143
144 /**@brief Return the distance squared between the ends of this and another vector
145 * This is symantically treating the vector like a point */
146 SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
147
148 /**@brief Return the distance between the ends of this and another vector
149 * This is symantically treating the vector like a point */
150 SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
151
152 SIMD_FORCE_INLINE btVector3& safeNormalize()
153 {
154 btVector3 absVec = this->absolute();
155 int32_t maxIndex = absVec.maxAxis();
156 if (absVec[maxIndex] > 0) {
157 *this /= absVec[maxIndex];
158 return * this /= length();
159 }
160 setValue(1, 0, 0);
161 return *this;
162 }
163
164 /**@brief Normalize this vector
165 * x^2 + y^2 + z^2 = 1 */
166 SIMD_FORCE_INLINE btVector3& normalize()
167 {
168 return * this /= length();
169 }
170
171 /**@brief Return a normalized version of this vector */
172 SIMD_FORCE_INLINE btVector3 normalized() const;
173
174 /**@brief Return a rotated version of this vector
175 * @param wAxis The axis to rotate about
176 * @param angle The angle to rotate by */
177 SIMD_FORCE_INLINE btVector3 rotate(const btVector3& wAxis, const btScalar angle) const;
178
179 /**@brief Return the angle between this and another vector
180 * @param v The other vector */
181 SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const
182 {
183 btScalar s = btSqrt(length2() * v.length2());
184 btFullAssert(s != btScalar(0.0));
185 return btAcos(dot(v) / s);
186 }
187 /**@brief Return a vector will the absolute values of each element */
188 SIMD_FORCE_INLINE btVector3 absolute() const
189 {
190 return btVector3(
191 btFabs(m_floats[0]),
192 btFabs(m_floats[1]),
193 btFabs(m_floats[2]));
194 }
195 /**@brief Return the cross product between this and another vector
196 * @param v The other vector */
197 SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
198 {
199 return btVector3(
200 m_floats[1] * v.m_floats[2] - m_floats[2] * v.m_floats[1],
201 m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
202 m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
203 }
204
205 SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
206 {
207 return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
208 }
209
210 /**@brief Return the axis with the smallest value
211 * Note return values are 0,1,2 for x, y, or z */
212 SIMD_FORCE_INLINE int32_t minAxis() const
213 {
214 return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
215 }
216
217 /**@brief Return the axis with the largest value
218 * Note return values are 0,1,2 for x, y, or z */
219 SIMD_FORCE_INLINE int32_t maxAxis() const
220 {
221 return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
222 }
223
224 SIMD_FORCE_INLINE int32_t furthestAxis() const
225 {
226 return absolute().minAxis();
227 }
228
229 SIMD_FORCE_INLINE int32_t closestAxis() const
230 {
231 return absolute().maxAxis();
232 }
233
234 SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
235 {
236 btScalar s = btScalar(1.0) - rt;
237 m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
238 m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
239 m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
240 //don't do the unused w component
241 // m_co[3] = s * v0[3] + rt * v1[3];
242 }
243
244 /**@brief Return the linear interpolation between this and another vector
245 * @param v The other vector
246 * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
247 SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
248 {
249 return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
250 m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
251 m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
252 }
253
254 /**@brief Elementwise multiply this vector by the other
255 * @param v The other vector */
256 SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
257 {
258 m_floats[0] *= v.m_floats[0];
259 m_floats[1] *= v.m_floats[1];
260 m_floats[2] *= v.m_floats[2];
261 return *this;
262 }
263
264 /**@brief Return the x value */
265 SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
266 /**@brief Return the y value */
267 SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
268 /**@brief Return the z value */
269 SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
270 /**@brief Set the x value */
271 SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x; };
272 /**@brief Set the y value */
273 SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y; };
274 /**@brief Set the z value */
275 SIMD_FORCE_INLINE void setZ(btScalar z) { m_floats[2] = z; };
276 /**@brief Set the w value */
277 SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w; };
278 /**@brief Return the x value */
279 SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
280 /**@brief Return the y value */
281 SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
282 /**@brief Return the z value */
283 SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
284 /**@brief Return the w value */
285 SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
286
287 //SIMD_FORCE_INLINE btScalar& operator[](int32_t i) { return (&m_floats[0])[i]; }
288 //SIMD_FORCE_INLINE const btScalar& operator[](int32_t i) const { return (&m_floats[0])[i]; }
289 ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
290 SIMD_FORCE_INLINE operator btScalar*() { return &m_floats[0]; }
291 SIMD_FORCE_INLINE operator const btScalar*() const { return &m_floats[0]; }
292
293 SIMD_FORCE_INLINE bool operator==(const btVector3& other) const
294 {
295 return ((m_floats[3] == other.m_floats[3]) && (m_floats[2] == other.m_floats[2]) && (m_floats[1] == other.m_floats[1]) && (m_floats[0] == other.m_floats[0]));
296 }
297
298 SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const
299 {
300 return !(*this == other);
301 }
302
303 /**@brief Set each element to the max of the current values and the values of another btVector3
304 * @param other The other btVector3 to compare with
305 */
306 SIMD_FORCE_INLINE void setMax(const btVector3& other)
307 {
308 btSetMax(m_floats[0], other.m_floats[0]);
309 btSetMax(m_floats[1], other.m_floats[1]);
310 btSetMax(m_floats[2], other.m_floats[2]);
311 btSetMax(m_floats[3], other.w());
312 }
313 /**@brief Set each element to the min of the current values and the values of another btVector3
314 * @param other The other btVector3 to compare with
315 */
316 SIMD_FORCE_INLINE void setMin(const btVector3& other)
317 {
318 btSetMin(m_floats[0], other.m_floats[0]);
319 btSetMin(m_floats[1], other.m_floats[1]);
320 btSetMin(m_floats[2], other.m_floats[2]);
321 btSetMin(m_floats[3], other.w());
322 }
323
324 SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z)
325 {
326 m_floats[0] = x;
327 m_floats[1] = y;
328 m_floats[2] = z;
329 m_floats[3] = btScalar(0.);
330 }
331
332 void getSkewSymmetricMatrix(btVector3 * v0, btVector3 * v1, btVector3 * v2) const
333 {
334 v0->setValue(0., -z(), y());
335 v1->setValue(z(), 0., -x());
336 v2->setValue(-y(), x(), 0.);
337 }
338
339 void setZero()
340 {
341 setValue(btScalar(0.), btScalar(0.), btScalar(0.));
342 }
343
344 SIMD_FORCE_INLINE bool isZero() const
345 {
346 return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
347 }
348
349 SIMD_FORCE_INLINE bool fuzzyZero() const
350 {
351 return length2() < SIMD_EPSILON;
352 }
353
354 SIMD_FORCE_INLINE void serialize(struct btVector3Data & dataOut) const;
355
356 SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn);
357
358 SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData & dataOut) const;
359
360 SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn);
361
362 SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData & dataOut) const;
363
364 SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn);
365};
366
367/**@brief Return the sum of two vectors (Point symantics)*/
368SIMD_FORCE_INLINE btVector3
369operator+(const btVector3& v1, const btVector3& v2)
370{
371 return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
372}
373
374/**@brief Return the elementwise product of two vectors */
375SIMD_FORCE_INLINE btVector3
376operator*(const btVector3& v1, const btVector3& v2)
377{
378 return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
379}
380
381/**@brief Return the difference between two vectors */
382SIMD_FORCE_INLINE btVector3
383operator-(const btVector3& v1, const btVector3& v2)
384{
385 return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
386}
387/**@brief Return the negative of the vector */
388SIMD_FORCE_INLINE btVector3
389operator-(const btVector3& v)
390{
391 return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
392}
393
394/**@brief Return the vector scaled by s */
395SIMD_FORCE_INLINE btVector3
396operator*(const btVector3& v, const btScalar& s)
397{
398 return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
399}
400
401/**@brief Return the vector scaled by s */
402SIMD_FORCE_INLINE btVector3
403operator*(const btScalar& s, const btVector3& v)
404{
405 return v * s;
406}
407
408/**@brief Return the vector inversely scaled by s */
409SIMD_FORCE_INLINE btVector3
410operator/(const btVector3& v, const btScalar& s)
411{
412 btFullAssert(s != btScalar(0.0));
413 return v * (btScalar(1.0) / s);
414}
415
416/**@brief Return the vector inversely scaled by s */
417SIMD_FORCE_INLINE btVector3
418operator/(const btVector3& v1, const btVector3& v2)
419{
420 return btVector3(v1.m_floats[0] / v2.m_floats[0], v1.m_floats[1] / v2.m_floats[1], v1.m_floats[2] / v2.m_floats[2]);
421}
422
423/**@brief Return the dot product between two vectors */
424SIMD_FORCE_INLINE btScalar
425btDot(const btVector3& v1, const btVector3& v2)
426{
427 return v1.dot(v2);
428}
429
430/**@brief Return the distance squared between two vectors */
431SIMD_FORCE_INLINE btScalar
432btDistance2(const btVector3& v1, const btVector3& v2)
433{
434 return v1.distance2(v2);
435}
436
437/**@brief Return the distance between two vectors */
438SIMD_FORCE_INLINE btScalar
439btDistance(const btVector3& v1, const btVector3& v2)
440{
441 return v1.distance(v2);
442}
443
444/**@brief Return the angle between two vectors */
445SIMD_FORCE_INLINE btScalar
446btAngle(const btVector3& v1, const btVector3& v2)
447{
448 return v1.angle(v2);
449}
450
451/**@brief Return the cross product of two vectors */
452SIMD_FORCE_INLINE btVector3
453btCross(const btVector3& v1, const btVector3& v2)
454{
455 return v1.cross(v2);
456}
457
458SIMD_FORCE_INLINE btScalar
459btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
460{
461 return v1.triple(v2, v3);
462}
463
464/**@brief Return the linear interpolation between two vectors
465 * @param v1 One vector
466 * @param v2 The other vector
467 * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
468SIMD_FORCE_INLINE btVector3
469lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
470{
471 return v1.lerp(v2, t);
472}
473
474SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
475{
476 return (v - *this).length2();
477}
478
479SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
480{
481 return (v - *this).length();
482}
483
484SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
485{
486 return *this / length();
487}
488
489SIMD_FORCE_INLINE btVector3 btVector3::rotate(const btVector3& wAxis, const btScalar angle) const
490{
491 // wAxis must be a unit lenght vector
492
493 btVector3 o = wAxis * wAxis.dot(*this);
494 btVector3 x = *this - o;
495 btVector3 y;
496
497 y = wAxis.cross(*this);
498
499 return (o + x * btCos(angle) + y * btSin(angle));
500}
501
502class btVector4 : public btVector3 {
503public:
504 SIMD_FORCE_INLINE btVector4() {}
505
506 SIMD_FORCE_INLINE btVector4(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w)
507 : btVector3(x, y, z)
508 {
509 m_floats[3] = w;
510 }
511
512 SIMD_FORCE_INLINE btVector4 absolute4() const
513 {
514 return btVector4(
515 btFabs(m_floats[0]),
516 btFabs(m_floats[1]),
517 btFabs(m_floats[2]),
518 btFabs(m_floats[3]));
519 }
520
521 btScalar getW() const { return m_floats[3]; }
522
523 SIMD_FORCE_INLINE int32_t maxAxis4() const
524 {
525 int32_t maxIndex = -1;
526 btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
527 if (m_floats[0] > maxVal) {
528 maxIndex = 0;
529 maxVal = m_floats[0];
530 }
531 if (m_floats[1] > maxVal) {
532 maxIndex = 1;
533 maxVal = m_floats[1];
534 }
535 if (m_floats[2] > maxVal) {
536 maxIndex = 2;
537 maxVal = m_floats[2];
538 }
539 if (m_floats[3] > maxVal) {
540 maxIndex = 3;
541 }
542 return maxIndex;
543 }
544
545 SIMD_FORCE_INLINE int32_t minAxis4() const
546 {
547 int32_t minIndex = -1;
548 btScalar minVal = btScalar(BT_LARGE_FLOAT);
549 if (m_floats[0] < minVal) {
550 minIndex = 0;
551 minVal = m_floats[0];
552 }
553 if (m_floats[1] < minVal) {
554 minIndex = 1;
555 minVal = m_floats[1];
556 }
557 if (m_floats[2] < minVal) {
558 minIndex = 2;
559 minVal = m_floats[2];
560 }
561 if (m_floats[3] < minVal) {
562 minIndex = 3;
563 }
564
565 return minIndex;
566 }
567
568 SIMD_FORCE_INLINE int32_t closestAxis4() const
569 {
570 return absolute4().maxAxis4();
571 }
572
573 /**@brief Set x,y,z and zero w
574 * @param x Value of x
575 * @param y Value of y
576 * @param z Value of z
577 */
578
579 /* void getValue(btScalar *m) const
580 {
581 m[0] = m_floats[0];
582 m[1] = m_floats[1];
583 m[2] =m_floats[2];
584 }
585*/
586 /**@brief Set the values
587 * @param x Value of x
588 * @param y Value of y
589 * @param z Value of z
590 * @param w Value of w
591 */
592 SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w)
593 {
594 m_floats[0] = x;
595 m_floats[1] = y;
596 m_floats[2] = z;
597 m_floats[3] = w;
598 }
599};
600
601///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
602SIMD_FORCE_INLINE void btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
603{
604#ifdef BT_USE_DOUBLE_PRECISION
605 unsigned char* dest = (unsigned char*)&destVal;
606 unsigned char* src = (unsigned char*)&sourceVal;
607 dest[0] = src[7];
608 dest[1] = src[6];
609 dest[2] = src[5];
610 dest[3] = src[4];
611 dest[4] = src[3];
612 dest[5] = src[2];
613 dest[6] = src[1];
614 dest[7] = src[0];
615#else
616 unsigned char* dest = (unsigned char*)&destVal;
617 unsigned char* src = (unsigned char*)&sourceVal;
618 dest[0] = src[3];
619 dest[1] = src[2];
620 dest[2] = src[1];
621 dest[3] = src[0];
622#endif //BT_USE_DOUBLE_PRECISION
623}
624///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
625SIMD_FORCE_INLINE void btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
626{
627 for (int32_t i = 0; i < 4; i++) {
628 btSwapScalarEndian(sourceVec[i], destVec[i]);
629 }
630}
631
632///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
633SIMD_FORCE_INLINE void btUnSwapVector3Endian(btVector3& vector)
634{
635
636 btVector3 swappedVec;
637 for (int32_t i = 0; i < 4; i++) {
638 btSwapScalarEndian(vector[i], swappedVec[i]);
639 }
640 vector = swappedVec;
641}
642
643template <class T>
644SIMD_FORCE_INLINE void btPlaneSpace1(const T& n, T& p, T& q)
645{
646 if (btFabs(n[2]) > SIMDSQRT12) {
647 // choose p in y-z plane
648 btScalar a = n[1] * n[1] + n[2] * n[2];
649 btScalar k = btRecipSqrt(a);
650 p[0] = 0;
651 p[1] = -n[2] * k;
652 p[2] = n[1] * k;
653 // set q = n x p
654 q[0] = a * k;
655 q[1] = -n[0] * p[2];
656 q[2] = n[0] * p[1];
657 }
658 else {
659 // choose p in x-y plane
660 btScalar a = n[0] * n[0] + n[1] * n[1];
661 btScalar k = btRecipSqrt(a);
662 p[0] = -n[1] * k;
663 p[1] = n[0] * k;
664 p[2] = 0;
665 // set q = n x p
666 q[0] = -n[2] * p[1];
667 q[1] = n[2] * p[0];
668 q[2] = a * k;
669 }
670}
671
672struct btVector3FloatData {
673 float m_floats[4];
674};
675
676struct btVector3DoubleData {
677 double m_floats[4];
678};
679
680SIMD_FORCE_INLINE void btVector3::serializeFloat(struct btVector3FloatData& dataOut) const
681{
682 ///could also do a memcpy, check if it is worth it
683 for (int32_t i = 0; i < 4; i++)
684 dataOut.m_floats[i] = float(m_floats[i]);
685}
686
687SIMD_FORCE_INLINE void btVector3::deSerializeFloat(const struct btVector3FloatData& dataIn)
688{
689 for (int32_t i = 0; i < 4; i++)
690 m_floats[i] = btScalar(dataIn.m_floats[i]);
691}
692
693SIMD_FORCE_INLINE void btVector3::serializeDouble(struct btVector3DoubleData& dataOut) const
694{
695 ///could also do a memcpy, check if it is worth it
696 for (int32_t i = 0; i < 4; i++)
697 dataOut.m_floats[i] = double(m_floats[i]);
698}
699
700SIMD_FORCE_INLINE void btVector3::deSerializeDouble(const struct btVector3DoubleData& dataIn)
701{
702 for (int32_t i = 0; i < 4; i++)
703 m_floats[i] = btScalar(dataIn.m_floats[i]);
704}
705
706SIMD_FORCE_INLINE void btVector3::serialize(struct btVector3Data& dataOut) const
707{
708 ///could also do a memcpy, check if it is worth it
709 for (int32_t i = 0; i < 4; i++)
710 dataOut.m_floats[i] = m_floats[i];
711}
712
713SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn)
714{
715 for (int32_t i = 0; i < 4; i++)
716 m_floats[i] = dataIn.m_floats[i];
717}
718
719// -- GODOT start --
720}; // namespace VHACD
721// -- GODOT end --
722
723#endif //BT_VECTOR3_H
724