1//
2// Redistribution and use in source and binary forms, with or without
3// modification, are permitted provided that the following conditions
4// are met:
5// * Redistributions of source code must retain the above copyright
6// notice, this list of conditions and the following disclaimer.
7// * Redistributions in binary form must reproduce the above copyright
8// notice, this list of conditions and the following disclaimer in the
9// documentation and/or other materials provided with the distribution.
10// * Neither the name of NVIDIA CORPORATION nor the names of its
11// contributors may be used to endorse or promote products derived
12// from this software without specific prior written permission.
13//
14// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
15// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
17// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
18// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
19// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
20// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
21// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
22// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
24// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25//
26// Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved.
27// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
28// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
29
30#ifndef DY_FEATHERSTONE_ARTICULATION_UTIL_H
31#define DY_FEATHERSTONE_ARTICULATION_UTIL_H
32
33#include "PsVecMath.h"
34#include "CmSpatialVector.h"
35#include "PsBitUtils.h"
36#include "foundation/PxMemory.h"
37
38namespace physx
39{
40
41namespace Dy
42{
43 static const size_t DY_MAX_DOF = 6;
44
45 struct SpatialSubspaceMatrix
46 {
47 static const PxU32 MaxColumns = 3;
48 public:
49
50 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialSubspaceMatrix() :numColumns(0)
51 {
52 //PxMemZero(columns, sizeof(Cm::SpatialVectorF) * 6);
53 memset(s: columns, c: 0, n: sizeof(Cm::UnAlignedSpatialVector) * MaxColumns);
54 }
55
56 PX_CUDA_CALLABLE PX_FORCE_INLINE void setNumColumns(const PxU32 nc)
57 {
58 numColumns = nc;
59 }
60
61 PX_CUDA_CALLABLE PX_FORCE_INLINE PxU32 getNumColumns() const
62 {
63 return numColumns;
64 }
65
66 PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF transposeMultiply(Cm::SpatialVectorF& v) const
67 {
68 PxReal result[6];
69 for (PxU32 i = 0; i < numColumns; ++i)
70 {
71 const Cm::UnAlignedSpatialVector& row = columns[i];
72 result[i] = row.dot(v);
73 }
74
75 Cm::SpatialVectorF res;
76 res.top.x = result[0]; res.top.y = result[1]; res.top.z = result[2];
77 res.bottom.x = result[3]; res.bottom.y = result[4]; res.bottom.z = result[5];
78
79 return res;
80
81 }
82
83 PX_CUDA_CALLABLE PX_FORCE_INLINE void setColumn(const PxU32 index, const PxVec3& top, const PxVec3& bottom)
84 {
85 PX_ASSERT(index < MaxColumns);
86 columns[index] = Cm::SpatialVectorF(top, bottom);
87 }
88
89 PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector& operator[](unsigned int num)
90 {
91 PX_ASSERT(num < MaxColumns);
92 return columns[num];
93 }
94
95 PX_CUDA_CALLABLE PX_FORCE_INLINE const Cm::UnAlignedSpatialVector& operator[](unsigned int num) const
96 {
97 PX_ASSERT(num < MaxColumns);
98 return columns[num];
99 }
100
101 PX_CUDA_CALLABLE PX_FORCE_INLINE const Cm::UnAlignedSpatialVector* getColumns() const
102 {
103 return columns;
104 }
105
106
107 private:
108 Cm::UnAlignedSpatialVector columns[MaxColumns]; //192 192
109 PxU32 numColumns; //4 208 (12 bytes padding)
110
111 };
112
113 //this should be 6x6 matrix
114 //|R, 0|
115 //|-R*rX, R|
116 struct SpatialTransform
117 {
118 PxMat33 R;
119 PxQuat q;
120 PxMat33 T;
121
122 public:
123 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform() : R(PxZero), T(PxZero)
124 {
125 }
126
127 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform(const PxMat33& R_, const PxMat33& T_) : R(R_), T(T_)
128 {
129 q = PxQuat(R_);
130 }
131
132 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform(const PxQuat& q_, const PxMat33& T_) : q(q_), T(T_)
133 {
134 R = PxMat33(q_);
135 }
136
137 //This assume angular is the top vector and linear is the bottom vector
138 /*PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVector operator *(const Cm::SpatialVector& s) const
139 {
140 const PxVec3 angular = R * s.angular;
141 const PxVec3 linear = T * s.angular + R * s.linear;
142 return Cm::SpatialVector(linear, angular);
143 }*/
144
145
146 ////This assume angular is the top vector and linear is the bottom vector
147 //PX_FORCE_INLINE Cm::SpatialVectorF operator *(Cm::SpatialVectorF& s) const
148 //{
149 // const PxVec3 top = R * s.top;
150 // const PxVec3 bottom = T * s.top + R * s.bottom;
151
152 // const PxVec3 top1 = q.rotate(s.top);
153 // const PxVec3 bottom1 = T * s.top + q.rotate(s.bottom);
154
155 ///* const PxVec3 tDif = (top - top1).abs();
156 // const PxVec3 bDif = (bottom - bottom1).abs();
157 // const PxReal eps = 0.001f;
158 // PX_ASSERT(tDif.x < eps && tDif.y < eps && tDif.z < eps);
159 // PX_ASSERT(bDif.x < eps && bDif.y < eps && bDif.z < eps);*/
160 // return Cm::SpatialVectorF(top1, bottom1);
161 //}
162
163 //This assume angular is the top vector and linear is the bottom vector
164 PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF operator *(const Cm::SpatialVectorF& s) const
165 {
166 //const PxVec3 top = R * s.top;
167 //const PxVec3 bottom = T * s.top + R * s.bottom;
168
169 const PxVec3 top1 = q.rotate(v: s.top);
170 const PxVec3 bottom1 = T * s.top + q.rotate(v: s.bottom);
171
172 return Cm::SpatialVectorF(top1, bottom1);
173 }
174
175 PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector operator *(const Cm::UnAlignedSpatialVector& s) const
176 {
177 //const PxVec3 top = R * s.top;
178 //const PxVec3 bottom = T * s.top + R * s.bottom;
179
180 const PxVec3 top1 = q.rotate(v: s.top);
181 const PxVec3 bottom1 = T * s.top + q.rotate(v: s.bottom);
182
183 return Cm::UnAlignedSpatialVector(top1, bottom1);
184 }
185
186 //transpose is the same as inverse, R(inverse) = R(transpose)
187 //|R(t), 0 |
188 //|rXR(t), R(t)|
189 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialTransform getTranspose() const
190 {
191 SpatialTransform ret;
192 ret.q = q.getConjugate();
193 ret.R = R.getTranspose();
194 ret.T = T.getTranspose();
195 return ret;
196
197 }
198
199 PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF transposeTransform(const Cm::SpatialVectorF& s) const
200 {
201 const PxVec3 top1 = q.rotateInv(v: s.top);
202 const PxVec3 bottom1 = T.transformTranspose(other: s.top) + q.rotateInv(v: s.bottom);
203
204 return Cm::SpatialVectorF(top1, bottom1);
205 }
206
207 PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector transposeTransform(const Cm::UnAlignedSpatialVector& s) const
208 {
209 const PxVec3 top1 = q.rotateInv(v: s.top);
210 const PxVec3 bottom1 = T.transformTranspose(other: s.top) + q.rotateInv(v: s.bottom);
211
212 return Cm::UnAlignedSpatialVector(top1, bottom1);
213 }
214
215 PX_CUDA_CALLABLE PX_FORCE_INLINE void operator =(SpatialTransform& other)
216 {
217 R = other.R;
218 q = other.q;
219 T = other.T;
220 }
221
222 };
223
224 struct InvStIs
225 {
226 PxReal invStIs[3][3];
227 };
228
229 struct IsInvD
230 {
231 Cm::SpatialVectorF isInvD[3];
232 };
233
234 //this should be 6x6 matrix and initialize to
235 //|0, M|
236 //|I, 0|
237 //this should be 6x6 matrix but bottomRight is the transpose of topLeft
238 //so we can get rid of bottomRight
239 struct SpatialMatrix
240 {
241 PxMat33 topLeft; // intialize to 0
242 PxMat33 topRight; // initialize to mass matrix
243 PxMat33 bottomLeft; // initialize to inertia
244 PxU32 padding; //4 112
245
246 public:
247
248 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix()
249 {
250 }
251
252 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix(PxZERO r) : topLeft(PxZero), topRight(PxZero),
253 bottomLeft(PxZero)
254 {
255 PX_UNUSED(r);
256 }
257
258 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix(const PxMat33& topLeft_, const PxMat33& topRight_, const PxMat33& bottomLeft_)
259 {
260 topLeft = topLeft_;
261 topRight = topRight_;
262 bottomLeft = bottomLeft_;
263 }
264
265 PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33 getBottomRight() const
266 {
267 return topLeft.getTranspose();
268 }
269
270 PX_FORCE_INLINE void setZero()
271 {
272 topLeft = PxMat33(0.f);
273 topRight = PxMat33(0.f);
274 bottomLeft = PxMat33(0.f);
275 }
276
277
278 //This assume angular is the top vector and linear is the bottom vector
279 PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVector operator *(const Cm::SpatialVector& s) const
280 {
281 const PxVec3 angular = topLeft * s.angular + topRight * s.linear;
282 const PxVec3 linear = bottomLeft * s.angular + topLeft.getTranspose() * s.linear;
283 return Cm::SpatialVector(linear, angular);
284 }
285
286 //This assume angular is the top vector and linear is the bottom vector
287 PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::SpatialVectorF operator *(const Cm::SpatialVectorF& s) const
288 {
289 const PxVec3 top = topLeft * s.top + topRight * s.bottom;
290 const PxVec3 bottom = bottomLeft * s.top + topLeft.transformTranspose(other: s.bottom);
291
292 return Cm::SpatialVectorF(top, bottom);
293 }
294
295 PX_CUDA_CALLABLE PX_FORCE_INLINE Cm::UnAlignedSpatialVector operator *(const Cm::UnAlignedSpatialVector& s) const
296 {
297 const PxVec3 top = topLeft * s.top + topRight * s.bottom;
298 const PxVec3 bottom = bottomLeft * s.top + topLeft.transformTranspose(other: s.bottom);
299
300 return Cm::UnAlignedSpatialVector(top, bottom);
301 }
302
303
304 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator *(const PxReal& s) const
305 {
306 const PxMat33 newTopLeft = topLeft * s;
307 const PxMat33 newTopRight = topRight * s;
308 const PxMat33 newBottomLeft = bottomLeft * s;
309
310 return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
311 }
312
313 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator -(const SpatialMatrix& s) const
314 {
315 PxMat33 newTopLeft = topLeft - s.topLeft;
316 PxMat33 newTopRight = topRight - s.topRight;
317 PxMat33 newBottomLeft = bottomLeft - s.bottomLeft;
318
319 return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
320 }
321
322 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator +(const SpatialMatrix& s) const
323 {
324 PxMat33 newTopLeft = topLeft + s.topLeft;
325 PxMat33 newTopRight = topRight + s.topRight;
326 PxMat33 newBottomLeft = bottomLeft + s.bottomLeft;
327
328 return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
329 }
330
331 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator-()
332 {
333 PxMat33 newTopLeft = -topLeft;
334 PxMat33 newTopRight = -topRight;
335 PxMat33 newBottomLeft = -bottomLeft;
336
337 return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
338 }
339
340 PX_CUDA_CALLABLE PX_FORCE_INLINE void operator +=(const SpatialMatrix& s)
341 {
342 topLeft += s.topLeft;
343 topRight += s.topRight;
344 bottomLeft += s.bottomLeft;
345 }
346
347 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix operator *(const SpatialMatrix& s)
348 {
349 PxMat33 sBottomRight = s.topLeft.getTranspose();
350 PxMat33 bottomRight = topLeft.getTranspose();
351
352 PxMat33 newTopLeft = topLeft * s.topLeft + topRight * s.bottomLeft;
353 PxMat33 newTopRight = topLeft * s.topRight + topRight * sBottomRight;
354 PxMat33 newBottomLeft = bottomLeft * s.topLeft + bottomRight * s.bottomLeft;
355
356 return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
357 }
358
359 static SpatialMatrix constructSpatialMatrix(const Cm::SpatialVector& Is, const Cm::SpatialVector& stI)
360 {
361 //construct top left
362 PxVec3 tLeftC0 = Is.angular * stI.angular.x;
363 PxVec3 tLeftC1 = Is.angular * stI.angular.y;
364 PxVec3 tLeftC2 = Is.angular * stI.angular.z;
365
366 PxMat33 topLeft(tLeftC0, tLeftC1, tLeftC2);
367
368 //construct top right
369 PxVec3 tRightC0 = Is.angular * stI.linear.x;
370 PxVec3 tRightC1 = Is.angular * stI.linear.y;
371 PxVec3 tRightC2 = Is.angular * stI.linear.z;
372 PxMat33 topRight(tRightC0, tRightC1, tRightC2);
373
374 //construct bottom left
375 PxVec3 bLeftC0 = Is.linear * stI.angular.x;
376 PxVec3 bLeftC1 = Is.linear * stI.angular.y;
377 PxVec3 bLeftC2 = Is.linear * stI.angular.z;
378 PxMat33 bottomLeft(bLeftC0, bLeftC1, bLeftC2);
379
380 return SpatialMatrix(topLeft, topRight, bottomLeft);
381 }
382
383 static PX_CUDA_CALLABLE SpatialMatrix constructSpatialMatrix(const Cm::SpatialVectorF& Is, const Cm::SpatialVectorF& stI)
384 {
385 //construct top left
386 PxVec3 tLeftC0 = Is.top * stI.top.x;
387 PxVec3 tLeftC1 = Is.top * stI.top.y;
388 PxVec3 tLeftC2 = Is.top * stI.top.z;
389
390 PxMat33 topLeft(tLeftC0, tLeftC1, tLeftC2);
391
392 //construct top right
393 PxVec3 tRightC0 = Is.top * stI.bottom.x;
394 PxVec3 tRightC1 = Is.top * stI.bottom.y;
395 PxVec3 tRightC2 = Is.top * stI.bottom.z;
396 PxMat33 topRight(tRightC0, tRightC1, tRightC2);
397
398 //construct bottom left
399 PxVec3 bLeftC0 = Is.bottom * stI.top.x;
400 PxVec3 bLeftC1 = Is.bottom * stI.top.y;
401 PxVec3 bLeftC2 = Is.bottom * stI.top.z;
402 PxMat33 bottomLeft(bLeftC0, bLeftC1, bLeftC2);
403
404 return SpatialMatrix(topLeft, topRight, bottomLeft);
405 }
406
407 static PX_CUDA_CALLABLE SpatialMatrix constructSpatialMatrix(const Cm::SpatialVectorF* columns)
408 {
409 PxMat33 topLeft(columns[0].top, columns[1].top, columns[2].top);
410 PxMat33 bottomLeft(columns[0].bottom, columns[1].bottom, columns[2].bottom);
411 PxMat33 topRight(columns[3].top, columns[4].top, columns[5].top);
412
413 return SpatialMatrix(topLeft, topRight, bottomLeft);
414 }
415
416 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix getTranspose()
417 {
418 PxMat33 newTopLeft = topLeft.getTranspose();
419 PxMat33 newTopRight = bottomLeft.getTranspose();
420 PxMat33 newBottomLeft = topRight.getTranspose();
421 //PxMat33 newBottomRight = bottomRight.getTranspose();
422
423 return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);// , newBottomRight);
424 }
425
426 //static bool isTranspose(const PxMat33& a, const PxMat33& b)
427 //{
428 // PxReal eps = 0.01f;
429 // //test bottomRight is the transpose of topLeft
430 // for (PxU32 i = 0; i <3; ++i)
431 // {
432 // for (PxU32 j = 0; j <3; ++j)
433 // {
434 // if (PxAbs(a[i][j] - b[j][i]) > eps)
435 // return false;
436 // }
437 // }
438
439 // return true;
440 //}
441
442 PX_FORCE_INLINE bool isIdentity(const PxMat33& matrix)
443 {
444 PxReal eps = 0.00001f;
445 float x = PxAbs(a: 1.f - matrix.column0.x);
446 float y = PxAbs(a: 1.f - matrix.column1.y);
447 float z = PxAbs(a: 1.f - matrix.column2.z);
448 bool identity = ((x < eps) && PxAbs(a: matrix.column0.y - 0.f) < eps && PxAbs(a: matrix.column0.z - 0.f) < eps) &&
449 (PxAbs(a: matrix.column1.x - 0.f) < eps && (y < eps) && PxAbs(a: matrix.column1.z - 0.f) < eps) &&
450 (PxAbs(a: matrix.column2.x - 0.f) < eps && PxAbs(a: matrix.column2.y - 0.f) < eps && (z < eps));
451
452 return identity;
453 }
454
455 PX_FORCE_INLINE bool isZero(const PxMat33& matrix)
456 {
457 PxReal eps = 0.0001f;
458 for (PxU32 i = 0; i < 3; ++i)
459 {
460 for (PxU32 j = 0; j < 3; ++j)
461 {
462 if (PxAbs(a: matrix[i][j]) > eps)
463 return false;
464 }
465 }
466
467 return true;
468 }
469
470 PX_FORCE_INLINE bool isIdentity()
471 {
472 bool topLeftIsIdentity = isIdentity(matrix: topLeft);
473
474 bool topRightIsZero = isZero(matrix: topRight);
475
476 bool bottomLeftIsZero = isZero(matrix: bottomLeft);
477
478 return topLeftIsIdentity && topRightIsZero && bottomLeftIsZero;
479 }
480
481 static bool isEqual(const PxMat33& s0, const PxMat33& s1)
482 {
483 PxReal eps = 0.00001f;
484 for (PxU32 i = 0; i < 3; ++i)
485 {
486 for (PxU32 j = 0; j < 3; ++j)
487 {
488 PxReal t = s0[i][j] - s1[i][j];
489 if (PxAbs(a: t) > eps)
490 return false;
491 }
492 }
493
494 return true;
495 }
496
497 PX_FORCE_INLINE bool isEqual(const SpatialMatrix& s)
498 {
499 bool topLeftEqual = isEqual(s0: topLeft, s1: s.topLeft);
500 bool topRightEqual = isEqual(s0: topRight, s1: s.topRight);
501 bool bottomLeftEqual = isEqual(s0: bottomLeft, s1: s.bottomLeft);
502
503 return topLeftEqual && topRightEqual && bottomLeftEqual;
504 }
505
506 static PX_CUDA_CALLABLE PX_FORCE_INLINE PxMat33 invertSym33(const PxMat33& in)
507 {
508 PxVec3 v0 = in[1].cross(v: in[2]),
509 v1 = in[2].cross(v: in[0]),
510 v2 = in[0].cross(v: in[1]);
511
512 PxReal det = v0.dot(v: in[0]);
513
514 if (det != 0)
515 {
516 PxReal recipDet = 1.0f / det;
517
518 return PxMat33(v0 * recipDet,
519 PxVec3(v0.y, v1.y, v1.z) * recipDet,
520 PxVec3(v0.z, v1.z, v2.z) * recipDet);
521 }
522 else
523 {
524 return PxMat33(PxIdentity);
525 }
526 }
527
528 static PX_FORCE_INLINE Ps::aos::Mat33V invertSym33(const Ps::aos::Mat33V& in)
529 {
530 using namespace Ps::aos;
531 const Vec3V v0 = V3Cross(a: in.col1, b: in.col2);
532 const Vec3V v1 = V3Cross(a: in.col2, b: in.col0);
533 const Vec3V v2 = V3Cross(a: in.col0, b: in.col1);
534
535 FloatV det = V3Dot(a: v0, b: in.col0);
536
537 const FloatV recipDet = FRecip(a: det);
538
539 if (!FAllEq(a: det, b: FZero()))
540 {
541 return Mat33V(V3Scale(a: v0, b: recipDet),
542 V3Scale(a: V3Merge(x: V3GetY(f: v0), y: V3GetY(f: v1), z: V3GetZ(f: v1)), b: recipDet),
543 V3Scale(a: V3Merge(x: V3GetZ(f: v0), y: V3GetZ(f: v1), z: V3GetZ(f: v2)), b: recipDet));
544 }
545 else
546 {
547 return Mat33V(V3UnitX(), V3UnitY(), V3UnitZ());
548 }
549
550 //return M33Inverse(in);
551 }
552
553 PX_CUDA_CALLABLE PX_FORCE_INLINE SpatialMatrix invertInertia()
554 {
555 PxMat33 aa = bottomLeft, ll = topRight, la = topLeft;
556
557 aa = (aa + aa.getTranspose())*0.5f;
558 ll = (ll + ll.getTranspose())*0.5f;
559
560 PxMat33 AAInv = invertSym33(in: aa);
561
562 PxMat33 z = -la * AAInv;
563 PxMat33 S = ll + z * la.getTranspose(); // Schur complement of mAA
564
565 PxMat33 LL = invertSym33(in: S);
566
567 PxMat33 LA = LL * z;
568 PxMat33 AA = AAInv + z.getTranspose() * LA;
569
570 SpatialMatrix result(LA.getTranspose(), AA, LL);// , LA);
571
572 return result;
573 }
574
575 PX_FORCE_INLINE void M33Store(const Ps::aos::Mat33V& src, PxMat33& dest)
576 {
577 Ps::aos::V3StoreU(a: src.col0, f&: dest.column0);
578 Ps::aos::V3StoreU(a: src.col1, f&: dest.column1);
579 Ps::aos::V3StoreU(a: src.col2, f&: dest.column2);
580 }
581
582 PX_FORCE_INLINE void invertInertiaV(SpatialMatrix& result)
583 {
584 using namespace Ps::aos;
585 Mat33V aa = M33Load(m: bottomLeft), ll = M33Load(m: topRight), la = M33Load(m: topLeft);
586
587 aa = M33Scale(a: M33Add(a: aa, b: M33Trnsps(a: aa)), b: FHalf());
588 ll = M33Scale(a: M33Add(a: ll, b: M33Trnsps(a: ll)), b: FHalf());
589
590 Mat33V AAInv = invertSym33(in: aa);
591
592 Mat33V z = M33MulM33(a: M33Neg(a: la), b: AAInv);
593 Mat33V S = M33Add(a: ll, b: M33MulM33(a: z, b: M33Trnsps(a: la))); // Schur complement of mAA
594
595 Mat33V LL = invertSym33(in: S);
596
597 Mat33V LA = M33MulM33(a: LL, b: z);
598 Mat33V AA = M33Add(a: AAInv, b: M33MulM33(a: M33Trnsps(a: z), b: LA));
599
600 M33Store(src: M33Trnsps(a: LA), dest&: result.topLeft);
601 M33Store(src: AA, dest&: result.topRight);
602 M33Store(src: LL, dest&: result.bottomLeft);
603 }
604
605 SpatialMatrix getInverse()
606 {
607 PxMat33 bottomRight = topLeft.getTranspose();
608
609 PxMat33 blInverse = bottomLeft.getInverse();
610 PxMat33 lComp0 = blInverse * (-bottomRight);
611 PxMat33 lComp1 = topLeft * lComp0 + topRight;
612
613 //This can be simplified
614 PxMat33 newBottomLeft = lComp1.getInverse();
615 PxMat33 newTopLeft = lComp0 * newBottomLeft;
616
617 PxMat33 trInverse = topRight.getInverse();
618 PxMat33 rComp0 = trInverse * (-topLeft);
619 PxMat33 rComp1 = bottomLeft + bottomRight * rComp0;
620
621 PxMat33 newTopRight = rComp1.getInverse();
622
623 return SpatialMatrix(newTopLeft, newTopRight, newBottomLeft);
624 }
625
626 void zero()
627 {
628 topLeft = PxMat33(PxZero);
629 topRight = PxMat33(PxZero);
630 bottomLeft = PxMat33(PxZero);
631 }
632
633 };
634
635 struct SpatialImpulseResponseMatrix
636 {
637 Cm::SpatialVectorF rows[6];
638
639 Cm::SpatialVectorF getResponse(const Cm::SpatialVectorF& impulse) const
640 {
641 /*return rows[0] * impulse.top.x + rows[1] * impulse.top.y + rows[2] * impulse.top.z
642 + rows[3] * impulse.bottom.x + rows[4] * impulse.bottom.y + rows[5] * impulse.bottom.z;*/
643
644 using namespace Ps::aos;
645 Cm::SpatialVectorV row0(V3LoadA(f: &rows[0].top.x), V3LoadA(f: &rows[0].bottom.x));
646 Cm::SpatialVectorV row1(V3LoadA(f: &rows[1].top.x), V3LoadA(f: &rows[1].bottom.x));
647 Cm::SpatialVectorV row2(V3LoadA(f: &rows[2].top.x), V3LoadA(f: &rows[2].bottom.x));
648 Cm::SpatialVectorV row3(V3LoadA(f: &rows[3].top.x), V3LoadA(f: &rows[3].bottom.x));
649 Cm::SpatialVectorV row4(V3LoadA(f: &rows[4].top.x), V3LoadA(f: &rows[4].bottom.x));
650 Cm::SpatialVectorV row5(V3LoadA(f: &rows[5].top.x), V3LoadA(f: &rows[5].bottom.x));
651
652 Vec4V top = V4LoadA(f: &impulse.top.x);
653 Vec4V bottom = V4LoadA(f: &impulse.bottom.x);
654
655 const FloatV ix = V4GetX(f: top);
656 const FloatV iy = V4GetY(f: top);
657 const FloatV iz = V4GetZ(f: top);
658 const FloatV ia = V4GetX(f: bottom);
659 const FloatV ib = V4GetY(f: bottom);
660 const FloatV ic = V4GetZ(f: bottom);
661
662 Cm::SpatialVectorV res = row0 * ix + row1 * iy + row2 * iz + row3 * ia + row4 * ib + row5 * ic;
663
664 Cm::SpatialVectorF returnVal;
665 V4StoreA(a: Vec4V_From_Vec3V(f: res.linear), f: &returnVal.top.x);
666 V4StoreA(a: Vec4V_From_Vec3V(f: res.angular), f: &returnVal.bottom.x);
667
668 return returnVal;
669
670 }
671
672 Cm::SpatialVectorV getResponse(const Cm::SpatialVectorV& impulse) const
673 {
674 using namespace Ps::aos;
675 Cm::SpatialVectorV row0(V3LoadA(f: &rows[0].top.x), V3LoadA(f: &rows[0].bottom.x));
676 Cm::SpatialVectorV row1(V3LoadA(f: &rows[1].top.x), V3LoadA(f: &rows[1].bottom.x));
677 Cm::SpatialVectorV row2(V3LoadA(f: &rows[2].top.x), V3LoadA(f: &rows[2].bottom.x));
678 Cm::SpatialVectorV row3(V3LoadA(f: &rows[3].top.x), V3LoadA(f: &rows[3].bottom.x));
679 Cm::SpatialVectorV row4(V3LoadA(f: &rows[4].top.x), V3LoadA(f: &rows[4].bottom.x));
680 Cm::SpatialVectorV row5(V3LoadA(f: &rows[5].top.x), V3LoadA(f: &rows[5].bottom.x));
681
682 const Vec3V top = impulse.linear;
683 const Vec3V bottom = impulse.angular;
684
685 const FloatV ix = V3GetX(f: top);
686 const FloatV iy = V3GetY(f: top);
687 const FloatV iz = V3GetZ(f: top);
688 const FloatV ia = V3GetX(f: bottom);
689 const FloatV ib = V3GetY(f: bottom);
690 const FloatV ic = V3GetZ(f: bottom);
691
692 Cm::SpatialVectorV res = row0 * ix + row1 * iy + row2 * iz + row3 * ia + row4 * ib + row5 * ic;
693 return res;
694 }
695 };
696
697 struct Temp6x6Matrix;
698
699 struct Temp6x3Matrix
700 {
701 PxReal column[3][6];
702 public:
703
704 Temp6x3Matrix()
705 {
706
707 }
708
709 Temp6x3Matrix(const Cm::SpatialVectorF* spatialAxis)
710 {
711 constructColumn(dest: column[0], v: spatialAxis[0]);
712 constructColumn(dest: column[1], v: spatialAxis[1]);
713 constructColumn(dest: column[2], v: spatialAxis[2]);
714 }
715
716 void constructColumn(PxReal* dest, const Cm::SpatialVectorF& v)
717 {
718 dest[0] = v.top.x;
719 dest[1] = v.top.y;
720 dest[2] = v.top.z;
721
722 dest[3] = v.bottom.x;
723 dest[4] = v.bottom.y;
724 dest[5] = v.bottom.z;
725 }
726
727 Temp6x6Matrix operator * (PxReal s[6][3]);
728
729 ////s is 3x6 matrix
730 //PX_FORCE_INLINE Temp6x6Matrix operator * (PxReal s[6][3])
731 //{
732 // Temp6x6Matrix temp;
733
734 // for (PxU32 i = 0; i < 6; ++i)
735 // {
736 // PxReal* tc = temp.column[i];
737
738 // for (PxU32 j = 0; j < 6; ++j)
739 // {
740 // tc[j] = 0.f;
741 // for (PxU32 k = 0; k < 3; ++k)
742 // {
743 // tc[j] += column[k][j] * s[i][k];
744 // }
745 // }
746 // }
747
748 // return temp;
749 //}
750
751 PX_FORCE_INLINE Temp6x3Matrix operator * (const PxMat33& s)
752 {
753 Temp6x3Matrix temp;
754
755 for (PxU32 i = 0; i < 3; ++i)
756 {
757 PxReal* tc = temp.column[i];
758 PxVec3 sc = s[i];
759
760 for (PxU32 j = 0; j < 6; ++j)
761 {
762 tc[j] = 0.f;
763 for (PxU32 k = 0; k < 3; ++k)
764 {
765 tc[j] += column[k][j] * sc[k];
766 }
767 }
768 }
769
770 return temp;
771 }
772
773 PX_FORCE_INLINE bool isColumnEqual(const PxU32 ind, const Cm::SpatialVectorF& col)
774 {
775 PxReal temp[6];
776 constructColumn(dest: temp, v: col);
777 const PxReal eps = 0.00001f;
778 for (PxU32 i = 0; i < 6; ++i)
779 {
780 const PxReal dif = column[ind][i] - temp[i];
781 if (PxAbs(a: dif) > eps)
782 return false;
783 }
784 return true;
785 }
786
787 };
788
789
790 struct Temp6x6Matrix
791 {
792 PxReal column[6][6];
793 public:
794 Temp6x6Matrix()
795 {
796
797 }
798
799 Temp6x6Matrix(const SpatialMatrix& spatialMatrix)
800 {
801 constructColumn(dest: column[0], top: spatialMatrix.topLeft.column0, bottom: spatialMatrix.bottomLeft.column0);
802 constructColumn(dest: column[1], top: spatialMatrix.topLeft.column1, bottom: spatialMatrix.bottomLeft.column1);
803 constructColumn(dest: column[2], top: spatialMatrix.topLeft.column2, bottom: spatialMatrix.bottomLeft.column2);
804
805 const PxMat33 bottomRight = spatialMatrix.getBottomRight();
806 constructColumn(dest: column[3], top: spatialMatrix.topRight.column0, bottom: bottomRight.column0);
807 constructColumn(dest: column[4], top: spatialMatrix.topRight.column1, bottom: bottomRight.column1);
808 constructColumn(dest: column[5], top: spatialMatrix.topRight.column2, bottom: bottomRight.column2);
809 }
810
811 void constructColumn(const PxU32 ind, const PxReal* const values)
812 {
813 for (PxU32 i = 0; i < 6; ++i)
814 {
815 column[ind][i] = values[i];
816 }
817 }
818
819 void constructColumn(PxReal* dest, const PxVec3& top, const PxVec3& bottom)
820 {
821 dest[0] = top.x;
822 dest[1] = top.y;
823 dest[2] = top.z;
824
825 dest[3] = bottom.x;
826 dest[4] = bottom.y;
827 dest[5] = bottom.z;
828 }
829
830 Temp6x6Matrix getTranspose() const
831 {
832 Temp6x6Matrix temp;
833 for (PxU32 i = 0; i < 6; ++i)
834 {
835 for (PxU32 j = 0; j < 6; ++j)
836 {
837 temp.column[i][j] = column[j][i];
838 }
839 }
840 return temp;
841 }
842
843 PX_FORCE_INLINE Cm::SpatialVector operator * (const Cm::SpatialVector& s) const
844 {
845 Temp6x6Matrix tempMatrix = getTranspose();
846 PxReal st[6];
847 st[0] = s.angular.x; st[1] = s.angular.y; st[2] = s.angular.z;
848 st[3] = s.linear.x; st[4] = s.linear.y; st[5] = s.linear.z;
849
850 PxReal result[6];
851 for (PxU32 i = 0; i < 6; i++)
852 {
853 result[i] = 0;
854 for (PxU32 j = 0; j < 6; ++j)
855 {
856 result[i] += tempMatrix.column[i][j] * st[j];
857 }
858 }
859
860
861 Cm::SpatialVector temp;
862 temp.angular.x = result[0]; temp.angular.y = result[1]; temp.angular.z = result[2];
863 temp.linear.x = result[3]; temp.linear.y = result[4]; temp.linear.z = result[5];
864 return temp;
865 }
866
867
868 PX_FORCE_INLINE Cm::SpatialVectorF operator * (const Cm::SpatialVectorF& s) const
869 {
870 PxReal st[6];
871 st[0] = s.top.x; st[1] = s.top.y; st[2] = s.top.z;
872 st[3] = s.bottom.x; st[4] = s.bottom.y; st[5] = s.bottom.z;
873
874 PxReal result[6];
875 for (PxU32 i = 0; i < 6; ++i)
876 {
877 result[i] = 0.f;
878 for (PxU32 j = 0; j < 6; ++j)
879 {
880 result[i] += column[j][i] * st[j];
881 }
882 }
883
884 Cm::SpatialVectorF temp;
885 temp.top.x = result[0]; temp.top.y = result[1]; temp.top.z = result[2];
886 temp.bottom.x = result[3]; temp.bottom.y = result[4]; temp.bottom.z = result[5];
887 return temp;
888 }
889
890 PX_FORCE_INLINE Temp6x3Matrix operator * (const Temp6x3Matrix& s) const
891 {
892 Temp6x3Matrix temp;
893 for (PxU32 i = 0; i < 3; ++i)
894 {
895 PxReal* result = temp.column[i];
896
897 const PxReal* input = s.column[i];
898
899 for (PxU32 j = 0; j < 6; ++j)
900 {
901 result[j] = 0.f;
902 for (PxU32 k = 0; k < 6; ++k)
903 {
904 result[j] += column[k][j] * input[k];
905 }
906 }
907 }
908
909 return temp;
910
911 }
912
913 PX_FORCE_INLINE Cm::SpatialVector spatialVectorMul(const Cm::SpatialVector& s)
914 {
915 PxReal st[6];
916 st[0] = s.angular.x; st[1] = s.angular.y; st[2] = s.angular.z;
917 st[3] = s.linear.x; st[4] = s.linear.y; st[5] = s.linear.z;
918
919 PxReal result[6];
920 for (PxU32 i = 0; i < 6; ++i)
921 {
922 result[i] = 0.f;
923 for (PxU32 j = 0; j < 6; j++)
924 {
925 result[i] += column[i][j] * st[j];
926 }
927 }
928
929 Cm::SpatialVector temp;
930 temp.angular.x = result[0]; temp.angular.y = result[1]; temp.angular.z = result[2];
931 temp.linear.x = result[3]; temp.linear.y = result[4]; temp.linear.z = result[5];
932 return temp;
933 }
934
935 PX_FORCE_INLINE bool isEqual(const Cm::SpatialVectorF* m)
936 {
937 PxReal temp[6];
938 PxReal eps = 0.00001f;
939 for (PxU32 i = 0; i < 6; ++i)
940 {
941 temp[0] = m[i].top.x; temp[1] = m[i].top.y; temp[2] = m[i].top.z;
942 temp[3] = m[i].bottom.x; temp[4] = m[i].bottom.y; temp[5] = m[i].bottom.z;
943
944 for (PxU32 j = 0; j < 6; ++j)
945 {
946 PxReal dif = column[i][j] - temp[j];
947 if (PxAbs(a: dif) > eps)
948 return false;
949 }
950 }
951
952 return true;
953 }
954 };
955
956 //s is 3x6 matrix
957 PX_FORCE_INLINE Temp6x6Matrix Temp6x3Matrix::operator * (PxReal s[6][3])
958 {
959 Temp6x6Matrix temp;
960
961 for (PxU32 i = 0; i < 6; ++i)
962 {
963 PxReal* tc = temp.column[i];
964
965 for (PxU32 j = 0; j < 6; ++j)
966 {
967 tc[j] = 0.f;
968 for (PxU32 k = 0; k < 3; ++k)
969 {
970 tc[j] += column[k][j] * s[i][k];
971 }
972 }
973 }
974
975 return temp;
976 }
977
978 PX_FORCE_INLINE void calculateNewVelocity(const PxTransform& newTransform, const PxTransform& oldTransform,
979 const PxReal dt, PxVec3& linear, PxVec3& angular)
980 {
981 //calculate the new velocity
982 linear = (newTransform.p - oldTransform.p) / dt;
983 PxQuat quat = newTransform.q * oldTransform.q.getConjugate();
984
985 if (quat.w < 0) //shortest angle.
986 quat = -quat;
987
988 PxReal angle;
989 PxVec3 axis;
990 quat.toRadiansAndUnitAxis(angle, axis);
991 angular = (axis * angle) / dt;
992 }
993
994
995 // generates a pair of quaternions (swing, twist) such that in = swing * twist, with
996 // swing.x = 0
997 // twist.y = twist.z = 0, and twist is a unit quat
998 PX_CUDA_CALLABLE PX_FORCE_INLINE void separateSwingTwist(const PxQuat& q, PxQuat& twist, PxQuat& swing1, PxQuat& swing2)
999 {
1000 twist = q.x != 0.0f ? PxQuat(q.x, 0, 0, q.w).getNormalized() : PxQuat(PxIdentity);
1001 PxQuat swing = q * twist.getConjugate();
1002 swing1 = swing.y != 0.f ? PxQuat(0.f, swing.y, 0.f, swing.w).getNormalized() : PxQuat(PxIdentity);
1003 swing = swing * swing1.getConjugate();
1004 swing2 = swing.z != 0.f ? PxQuat(0.f, 0.f, swing.z, swing.w).getNormalized() : PxQuat(PxIdentity);
1005 }
1006
1007 PX_CUDA_CALLABLE PX_FORCE_INLINE void separateSwingTwist2(const PxQuat& q, PxQuat& twist, PxQuat& swing1, PxQuat& swing2)
1008 {
1009 swing2 = q.z != 0.0f ? PxQuat(0.f, 0.f, q.z, q.w).getNormalized() : PxQuat(PxIdentity);
1010 PxQuat swing = q * swing2.getConjugate();
1011 swing1 = swing.y != 0.f ? PxQuat(0.f, swing.y, 0.f, swing.w).getNormalized() : PxQuat(PxIdentity);
1012 swing = swing * swing1.getConjugate();
1013 twist = swing.x != 0.f ? PxQuat(swing.x, 0.f, 0.f, swing.w).getNormalized() : PxQuat(PxIdentity);
1014 }
1015
1016
1017} //namespace Dy
1018
1019}
1020
1021#endif
1022

source code of qtquick3dphysics/src/3rdparty/PhysX/source/lowleveldynamics/include/DyFeatherstoneArticulationUtils.h