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 PXFOUNDATION_PXTRANSFORM_H |
31 | #define PXFOUNDATION_PXTRANSFORM_H |
32 | /** \addtogroup foundation |
33 | @{ |
34 | */ |
35 | |
36 | #include "foundation/PxQuat.h" |
37 | #include "foundation/PxPlane.h" |
38 | |
39 | #if !PX_DOXYGEN |
40 | namespace physx |
41 | { |
42 | #endif |
43 | |
44 | /*! |
45 | \brief class representing a rigid euclidean transform as a quaternion and a vector |
46 | */ |
47 | |
48 | class PxTransform |
49 | { |
50 | public: |
51 | PxQuat q; |
52 | PxVec3 p; |
53 | |
54 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxTransform() |
55 | { |
56 | } |
57 | |
58 | PX_CUDA_CALLABLE PX_FORCE_INLINE explicit PxTransform(const PxVec3& position) : q(PxIdentity), p(position) |
59 | { |
60 | } |
61 | |
62 | PX_CUDA_CALLABLE PX_FORCE_INLINE explicit PxTransform(PxIDENTITY r) : q(PxIdentity), p(PxZero) |
63 | { |
64 | PX_UNUSED(r); |
65 | } |
66 | |
67 | PX_CUDA_CALLABLE PX_FORCE_INLINE explicit PxTransform(const PxQuat& orientation) : q(orientation), p(0) |
68 | { |
69 | PX_SHARED_ASSERT(orientation.isSane()); |
70 | } |
71 | |
72 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxTransform(float x, float y, float z, PxQuat aQ = PxQuat(PxIdentity)) |
73 | : q(aQ), p(x, y, z) |
74 | { |
75 | } |
76 | |
77 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxTransform(const PxVec3& p0, const PxQuat& q0) : q(q0), p(p0) |
78 | { |
79 | PX_SHARED_ASSERT(q0.isSane()); |
80 | } |
81 | |
82 | PX_CUDA_CALLABLE PX_FORCE_INLINE explicit PxTransform(const PxMat44& m); // defined in PxMat44.h |
83 | |
84 | /** |
85 | \brief returns true if the two transforms are exactly equal |
86 | */ |
87 | PX_CUDA_CALLABLE PX_INLINE bool operator==(const PxTransform& t) const |
88 | { |
89 | return p == t.p && q == t.q; |
90 | } |
91 | |
92 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxTransform operator*(const PxTransform& x) const |
93 | { |
94 | PX_SHARED_ASSERT(x.isSane()); |
95 | return transform(src: x); |
96 | } |
97 | |
98 | //! Equals matrix multiplication |
99 | PX_CUDA_CALLABLE PX_INLINE PxTransform& operator*=(PxTransform& other) |
100 | { |
101 | *this = *this * other; |
102 | return *this; |
103 | } |
104 | |
105 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxTransform getInverse() const |
106 | { |
107 | PX_SHARED_ASSERT(isFinite()); |
108 | return PxTransform(q.rotateInv(v: -p), q.getConjugate()); |
109 | } |
110 | |
111 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 transform(const PxVec3& input) const |
112 | { |
113 | PX_SHARED_ASSERT(isFinite()); |
114 | return q.rotate(v: input) + p; |
115 | } |
116 | |
117 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 transformInv(const PxVec3& input) const |
118 | { |
119 | PX_SHARED_ASSERT(isFinite()); |
120 | return q.rotateInv(v: input - p); |
121 | } |
122 | |
123 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 rotate(const PxVec3& input) const |
124 | { |
125 | PX_SHARED_ASSERT(isFinite()); |
126 | return q.rotate(v: input); |
127 | } |
128 | |
129 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxVec3 rotateInv(const PxVec3& input) const |
130 | { |
131 | PX_SHARED_ASSERT(isFinite()); |
132 | return q.rotateInv(v: input); |
133 | } |
134 | |
135 | //! Transform transform to parent (returns compound transform: first src, then *this) |
136 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxTransform transform(const PxTransform& src) const |
137 | { |
138 | PX_SHARED_ASSERT(src.isSane()); |
139 | PX_SHARED_ASSERT(isSane()); |
140 | // src = [srct, srcr] -> [r*srct + t, r*srcr] |
141 | return PxTransform(q.rotate(v: src.p) + p, q * src.q); |
142 | } |
143 | |
144 | /** |
145 | \brief returns true if finite and q is a unit quaternion |
146 | */ |
147 | |
148 | PX_CUDA_CALLABLE bool isValid() const |
149 | { |
150 | return p.isFinite() && q.isFinite() && q.isUnit(); |
151 | } |
152 | |
153 | /** |
154 | \brief returns true if finite and quat magnitude is reasonably close to unit to allow for some accumulation of error |
155 | vs isValid |
156 | */ |
157 | |
158 | PX_CUDA_CALLABLE bool isSane() const |
159 | { |
160 | return isFinite() && q.isSane(); |
161 | } |
162 | |
163 | /** |
164 | \brief returns true if all elems are finite (not NAN or INF, etc.) |
165 | */ |
166 | PX_CUDA_CALLABLE PX_FORCE_INLINE bool isFinite() const |
167 | { |
168 | return p.isFinite() && q.isFinite(); |
169 | } |
170 | |
171 | //! Transform transform from parent (returns compound transform: first src, then this->inverse) |
172 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxTransform transformInv(const PxTransform& src) const |
173 | { |
174 | PX_SHARED_ASSERT(src.isSane()); |
175 | PX_SHARED_ASSERT(isFinite()); |
176 | // src = [srct, srcr] -> [r^-1*(srct-t), r^-1*srcr] |
177 | PxQuat qinv = q.getConjugate(); |
178 | return PxTransform(qinv.rotate(v: src.p - p), qinv * src.q); |
179 | } |
180 | |
181 | /** |
182 | \brief transform plane |
183 | */ |
184 | |
185 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxPlane transform(const PxPlane& plane) const |
186 | { |
187 | PxVec3 transformedNormal = rotate(input: plane.n); |
188 | return PxPlane(transformedNormal, plane.d - p.dot(v: transformedNormal)); |
189 | } |
190 | |
191 | /** |
192 | \brief inverse-transform plane |
193 | */ |
194 | |
195 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxPlane inverseTransform(const PxPlane& plane) const |
196 | { |
197 | PxVec3 transformedNormal = rotateInv(input: plane.n); |
198 | return PxPlane(transformedNormal, plane.d + p.dot(v: plane.n)); |
199 | } |
200 | |
201 | /** |
202 | \brief return a normalized transform (i.e. one in which the quaternion has unit magnitude) |
203 | */ |
204 | PX_CUDA_CALLABLE PX_FORCE_INLINE PxTransform getNormalized() const |
205 | { |
206 | return PxTransform(p, q.getNormalized()); |
207 | } |
208 | }; |
209 | |
210 | #if !PX_DOXYGEN |
211 | } // namespace physx |
212 | #endif |
213 | |
214 | /** @} */ |
215 | #endif // #ifndef PXFOUNDATION_PXTRANSFORM_H |
216 | |