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 PSFOUNDATION_PSVECTRANSFORM_H
31#define PSFOUNDATION_PSVECTRANSFORM_H
32
33#include "PsVecMath.h"
34#include "foundation/PxTransform.h"
35
36namespace physx
37{
38namespace shdfnd
39{
40namespace aos
41{
42
43class PsTransformV
44{
45 public:
46 QuatV q;
47 Vec3V p;
48
49 PX_FORCE_INLINE PsTransformV(const PxTransform& orientation)
50 {
51 // const PxQuat oq = orientation.q;
52 // const PxF32 f[4] = {oq.x, oq.y, oq.z, oq.w};
53 q = QuatVLoadXYZW(x: orientation.q.x, y: orientation.q.y, z: orientation.q.z, w: orientation.q.w);
54 // q = QuatV_From_F32Array(&oq.x);
55 p = V3LoadU(f: orientation.p);
56 }
57
58 PX_FORCE_INLINE PsTransformV(const Vec3VArg p0 = V3Zero(), const QuatVArg q0 = QuatIdentity()) : q(q0), p(p0)
59 {
60 PX_ASSERT(isSaneQuatV(q0));
61 }
62
63 PX_FORCE_INLINE PsTransformV operator*(const PsTransformV& x) const
64 {
65 PX_ASSERT(x.isSane());
66 return transform(src: x);
67 }
68
69 PX_FORCE_INLINE PsTransformV getInverse() const
70 {
71 PX_ASSERT(isFinite());
72 // return PxTransform(q.rotateInv(-p),q.getConjugate());
73 return PsTransformV(QuatRotateInv(q, v: V3Neg(f: p)), QuatConjugate(q));
74 }
75
76 PX_FORCE_INLINE void normalize()
77 {
78 p = V3Zero();
79 q = QuatIdentity();
80 }
81
82 PX_FORCE_INLINE void Invalidate()
83 {
84 p = V3Splat(f: FMax());
85 q = QuatIdentity();
86 }
87
88 PX_FORCE_INLINE Vec3V transform(const Vec3VArg input) const
89 {
90 PX_ASSERT(isFinite());
91 // return q.rotate(input) + p;
92 return QuatTransform(q, p, v: input);
93 }
94
95 PX_FORCE_INLINE Vec3V transformInv(const Vec3VArg input) const
96 {
97 PX_ASSERT(isFinite());
98 // return q.rotateInv(input-p);
99 return QuatRotateInv(q, v: V3Sub(a: input, b: p));
100 }
101
102 PX_FORCE_INLINE Vec3V rotate(const Vec3VArg input) const
103 {
104 PX_ASSERT(isFinite());
105 // return q.rotate(input);
106 return QuatRotate(q, v: input);
107 }
108
109 PX_FORCE_INLINE Vec3V rotateInv(const Vec3VArg input) const
110 {
111 PX_ASSERT(isFinite());
112 // return q.rotateInv(input);
113 return QuatRotateInv(q, v: input);
114 }
115
116 //! Transform transform to parent (returns compound transform: first src, then *this)
117 PX_FORCE_INLINE PsTransformV transform(const PsTransformV& src) const
118 {
119 PX_ASSERT(src.isSane());
120 PX_ASSERT(isSane());
121 // src = [srct, srcr] -> [r*srct + t, r*srcr]
122 // return PxTransform(q.rotate(src.p) + p, q*src.q);
123 return PsTransformV(V3Add(a: QuatRotate(q, v: src.p), b: p), QuatMul(a: q, b: src.q));
124 }
125
126 /**
127 \brief returns true if finite and q is a unit quaternion
128 */
129
130 PX_FORCE_INLINE bool isValid() const
131 {
132 // return p.isFinite() && q.isFinite() && q.isValid();
133 return isFiniteVec3V(a: p) & isFiniteQuatV(q) & isValidQuatV(q);
134 }
135
136 /**
137 \brief returns true if finite and quat magnitude is reasonably close to unit to allow for some accumulation of error
138 vs isValid
139 */
140
141 PX_FORCE_INLINE bool isSane() const
142 {
143 // return isFinite() && q.isSane();
144 return isFinite() & isSaneQuatV(q);
145 }
146
147 /**
148 \brief returns true if all elems are finite (not NAN or INF, etc.)
149 */
150 PX_FORCE_INLINE bool isFinite() const
151 {
152 // return p.isFinite() && q.isFinite();
153 return isFiniteVec3V(a: p) & isFiniteQuatV(q);
154 }
155
156 //! Transform transform from parent (returns compound transform: first src, then this->inverse)
157 PX_FORCE_INLINE PsTransformV transformInv(const PsTransformV& src) const
158 {
159 PX_ASSERT(src.isSane());
160 PX_ASSERT(isFinite());
161 // src = [srct, srcr] -> [r^-1*(srct-t), r^-1*srcr]
162 /*PxQuat qinv = q.getConjugate();
163 return PxTransform(qinv.rotate(src.p - p), qinv*src.q);*/
164 const QuatV qinv = QuatConjugate(q);
165 const Vec3V v = QuatRotate(q: qinv, v: V3Sub(a: src.p, b: p));
166 const QuatV rot = QuatMul(a: qinv, b: src.q);
167 return PsTransformV(v, rot);
168 }
169
170 static PX_FORCE_INLINE PsTransformV createIdentity()
171 {
172 return PsTransformV(V3Zero());
173 }
174};
175
176PX_FORCE_INLINE PsTransformV loadTransformA(const PxTransform& transform)
177{
178 const QuatV q0 = QuatVLoadA(v: &transform.q.x);
179 const Vec3V p0 = V3LoadA(f: &transform.p.x);
180
181 return PsTransformV(p0, q0);
182}
183
184PX_FORCE_INLINE PsTransformV loadTransformU(const PxTransform& transform)
185{
186 const QuatV q0 = QuatVLoadU(v: &transform.q.x);
187 const Vec3V p0 = V3LoadU(i: &transform.p.x);
188
189 return PsTransformV(p0, q0);
190}
191
192class PsMatTransformV
193{
194 public:
195 Mat33V rot;
196 Vec3V p;
197
198 PX_FORCE_INLINE PsMatTransformV()
199 {
200 p = V3Zero();
201 rot = M33Identity();
202 }
203 PX_FORCE_INLINE PsMatTransformV(const Vec3VArg _p, const Mat33V& _rot)
204 {
205 p = _p;
206 rot = _rot;
207 }
208
209 PX_FORCE_INLINE PsMatTransformV(const PsTransformV& other)
210 {
211 p = other.p;
212 QuatGetMat33V(q: other.q, column0&: rot.col0, column1&: rot.col1, column2&: rot.col2);
213 }
214
215 PX_FORCE_INLINE PsMatTransformV(const Vec3VArg _p, const QuatV& quat)
216 {
217 p = _p;
218 QuatGetMat33V(q: quat, column0&: rot.col0, column1&: rot.col1, column2&: rot.col2);
219 }
220
221 PX_FORCE_INLINE Vec3V getCol0() const
222 {
223 return rot.col0;
224 }
225
226 PX_FORCE_INLINE Vec3V getCol1() const
227 {
228 return rot.col1;
229 }
230
231 PX_FORCE_INLINE Vec3V getCol2() const
232 {
233 return rot.col2;
234 }
235
236 PX_FORCE_INLINE void setCol0(const Vec3VArg col0)
237 {
238 rot.col0 = col0;
239 }
240
241 PX_FORCE_INLINE void setCol1(const Vec3VArg col1)
242 {
243 rot.col1 = col1;
244 }
245
246 PX_FORCE_INLINE void setCol2(const Vec3VArg col2)
247 {
248 rot.col2 = col2;
249 }
250
251 PX_FORCE_INLINE Vec3V transform(const Vec3VArg input) const
252 {
253 return V3Add(a: p, b: M33MulV3(a: rot, b: input));
254 }
255
256 PX_FORCE_INLINE Vec3V transformInv(const Vec3VArg input) const
257 {
258 return M33TrnspsMulV3(a: rot, b: V3Sub(a: input, b: p)); // QuatRotateInv(q, V3Sub(input, p));
259 }
260
261 PX_FORCE_INLINE Vec3V rotate(const Vec3VArg input) const
262 {
263 return M33MulV3(a: rot, b: input);
264 }
265
266 PX_FORCE_INLINE Vec3V rotateInv(const Vec3VArg input) const
267 {
268 return M33TrnspsMulV3(a: rot, b: input);
269 }
270
271 PX_FORCE_INLINE PsMatTransformV transformInv(const PsMatTransformV& src) const
272 {
273
274 const Vec3V v = M33TrnspsMulV3(a: rot, b: V3Sub(a: src.p, b: p));
275 const Mat33V mat = M33MulM33(a: M33Trnsps(a: rot), b: src.rot);
276 return PsMatTransformV(v, mat);
277 }
278};
279}
280}
281}
282
283#endif
284

source code of qtquick3dphysics/src/3rdparty/PhysX/source/foundation/include/PsVecTransform.h