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
31#ifndef PX_PHYSICS_NX_ARTICULATION_RC
32#define PX_PHYSICS_NX_ARTICULATION_RC
33/** \addtogroup physics
34@{ */
35
36#include "PxArticulationBase.h"
37#include "foundation/PxVec3.h"
38#include "foundation/PxTransform.h"
39#include "solver/PxSolverDefs.h"
40
41#if !PX_DOXYGEN
42namespace physx
43{
44#endif
45
46 PX_ALIGN_PREFIX(16)
47 struct PxSpatialForce
48 {
49 PxVec3 force;
50 PxReal pad0;
51 PxVec3 torque;
52 PxReal pad1;
53 }
54 PX_ALIGN_SUFFIX(16);
55
56 PX_ALIGN_PREFIX(16)
57 struct PxSpatialVelocity
58 {
59 PxVec3 linear;
60 PxReal pad0;
61 PxVec3 angular;
62 PxReal pad1;
63 }
64 PX_ALIGN_SUFFIX(16);
65
66 class PxJoint;
67
68 struct PxArticulationRootLinkData
69 {
70 PxTransform transform;
71 PxVec3 worldLinVel;
72 PxVec3 worldAngVel;
73 PxVec3 worldLinAccel;
74 PxVec3 worldAngAccel;
75 };
76
77 class PxArticulationCache
78 {
79 public:
80 enum Enum
81 {
82 eVELOCITY = (1 << 0), //!< The joint velocities this frame. Note, this is the accumulated joint velocities, not change in joint velocity.
83 eACCELERATION = (1 << 1), //!< The joint accelerations this frame. Delta velocity can be computed from acceleration * dt.
84 ePOSITION = (1 << 2), //!< The joint positions this frame. Note, this is the accumulated joint positions over frames, not change in joint position.
85 eFORCE = (1 << 3), //!< The joint forces this frame. Note, the application should provide these values for the forward dynamic. If the application is using inverse dynamic, this is the joint force returned.
86 eLINKVELOCITY = (1 << 4), //!< The link velocities this frame.
87 eLINKACCELERATION = (1 << 5), //!< The link accelerations this frame.
88 eROOT = (1 << 6), //!< Root link transform, velocity and acceleration. Note, when the application call applyCache with eROOT flag, it won't apply root link's acceleration to the simulation
89 eALL = (eVELOCITY | eACCELERATION | ePOSITION| eLINKVELOCITY | eLINKACCELERATION | eROOT )
90 };
91 PxArticulationCache() :
92 externalForces (NULL),
93 denseJacobian (NULL),
94 massMatrix (NULL),
95 jointVelocity (NULL),
96 jointAcceleration (NULL),
97 jointPosition (NULL),
98 jointForce (NULL),
99 rootLinkData (NULL),
100 coefficientMatrix (NULL),
101 lambda (NULL),
102 scratchMemory (NULL),
103 scratchAllocator (NULL),
104 version (0)
105 {}
106
107 PxSpatialForce* externalForces; // N = getNbLinks()
108 PxReal* denseJacobian; // N = 6*getDofs()*NumJoints, NumJoints = getNbLinks() - 1
109 PxReal* massMatrix; // N = getDofs()*getDofs()
110 PxReal* jointVelocity; // N = getDofs()
111 PxReal* jointAcceleration; // N = getDofs()
112 PxReal* jointPosition; // N = getDofs()
113 PxReal* jointForce; // N = getDofs()
114 PxSpatialVelocity* linkVelocity; // N = getNbLinks()
115 PxSpatialVelocity* linkAcceleration; // N = getNbLinks()
116 PxArticulationRootLinkData* rootLinkData; // root link Data
117
118 //application need to allocate those memory and assign them to the cache
119 PxReal* coefficientMatrix;
120 PxReal* lambda;
121
122 //These three members won't be set to zero when zeroCache get called
123 void* scratchMemory; //this is used for internal calculation
124 void* scratchAllocator;
125 PxU32 version; //cache version. If the articulation configuration change, the cache is invalid
126 };
127
128 typedef PxFlags<PxArticulationCache::Enum, PxU8> PxArticulationCacheFlags;
129 PX_FLAGS_OPERATORS(PxArticulationCache::Enum, PxU8)
130
131 /**
132 \brief a tree structure of bodies connected by joints that is treated as a unit by the dynamics solver
133
134 Articulations are more expensive to simulate than the equivalent collection of
135 PxRigidDynamic and PxJoint structures, but because the dynamics solver treats
136 each articulation as a single object, they are much less prone to separation and
137 have better support for actuation. An articulation may have at most 64 links.
138
139 @see PxArticulationJoint PxArticulationLink PxPhysics.createArticulation
140 */
141
142#if PX_VC
143#pragma warning(push)
144#pragma warning(disable : 4435)
145#endif
146
147 class PxArticulationReducedCoordinate : public PxArticulationBase
148 {
149 public:
150
151 virtual void release() = 0;
152
153 /**
154 \brief Sets flags on the articulation
155
156 \param[in] flags Articulation flags
157
158 */
159 virtual void setArticulationFlags(PxArticulationFlags flags) = 0;
160
161 /**
162 \brief Raises or clears a flag on the articulation
163
164 \param[in] flag The articulation flag
165 \param[in] value true/false indicating whether to raise or clear the flag
166
167 */
168 virtual void setArticulationFlag(PxArticulationFlag::Enum flag, bool value) = 0;
169
170 /**
171 \brief return PxArticulationFlags
172 */
173 virtual PxArticulationFlags getArticulationFlags() const = 0;
174
175 /**
176 \brief returns the total Dofs of the articulation
177 */
178 virtual PxU32 getDofs() const = 0;
179
180 /**
181 \brief create an articulation cache
182
183 \note this call may only be made on articulations that are in a scene, and may not be made during simulation
184 */
185 virtual PxArticulationCache* createCache() const = 0;
186
187 /**
188 \brief Get the size of the articulation cache
189
190 \note this call may only be made on articulations that are in a scene, and may not be made during simulation
191 */
192 virtual PxU32 getCacheDataSize() const = 0;
193
194 /**
195 \brief zero all data in the articulation cache beside the cache version
196
197 \note this call may only be made on articulations that are in a scene, and may not be made during simulation
198 */
199 virtual void zeroCache(PxArticulationCache& cache) = 0;
200
201 /**
202 \brief apply the user defined data in the cache to the articulation system
203
204 \param[in] cache articulation data.
205 \param[in] flag The mode to use when determine which value in the cache will be applied to the articulation
206 \param[in] autowake Specify if the call should wake up the articulation if it is currently asleep. If true and the current wake counter value is smaller than #PxSceneDesc::wakeCounterResetValue it will get increased to the reset value.
207
208 @see createCache copyInternalStateToCache
209 */
210 virtual void applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag, bool autowake = true) = 0;
211
212 /**
213 \brief copy the internal data of the articulation to the cache
214
215 \param[in] cache articulation data
216 \param[in] flag this indicates what kind of data the articulation system need to copy to the cache
217
218 @see createCache applyCache
219 */
220 virtual void copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const = 0;
221
222 /**
223 \brief release an articulation cache
224
225 \param[in] cache the cache to release
226
227 @see createCache applyCache copyInternalStateToCache
228 */
229 virtual void releaseCache(PxArticulationCache& cache) const = 0;
230
231 /**
232 \brief reduce the maximum data format to the reduced internal data
233 \param[in] maximum joint data format
234 \param[out] reduced joint data format
235 */
236 virtual void packJointData(const PxReal* maximum, PxReal* reduced) const = 0;
237
238 /**
239 \brief turn the reduced internal data to maximum joint data format
240 \param[in] reduced joint data format
241 \param[out] maximum joint data format
242 */
243 virtual void unpackJointData(const PxReal* reduced, PxReal* maximum) const = 0;
244
245 /**
246 \brief initialize all the common data for inverse dynamics
247 */
248 virtual void commonInit() const = 0;
249
250 /**
251 \brief determine the statically balance of the joint force of gravity for entire articulation. External force, joint velocity and joint acceleration
252 are set to zero, the joint force returned will be purely determined by gravity.
253
254 \param[out] cache return joint forces which can counteract gravity force
255
256 @see commonInit
257 */
258 virtual void computeGeneralizedGravityForce(PxArticulationCache& cache) const = 0;
259
260 /**
261 \brief determine coriolise and centrifugal force. External force, gravity and joint acceleration
262 are set to zero, the joint force return will be coriolise and centrifugal force for each joint.
263
264 \param[in] cache data
265
266 @see commonInit
267 */
268 virtual void computeCoriolisAndCentrifugalForce(PxArticulationCache& cache) const = 0;
269
270 /**
271 \brief determine joint force change caused by external force. Gravity, joint acceleration and joint velocity
272 are all set to zero.
273
274 \param[in] cache data
275
276 @see commonInit
277 */
278 virtual void computeGeneralizedExternalForce(PxArticulationCache& cache) const = 0;
279
280 /**
281 \brief determine the joint acceleration for each joint
282 This is purely calculates the change in joint acceleration due to change in the joint force
283
284 \param[in] cache articulation data
285
286 @see commonInit
287 */
288 virtual void computeJointAcceleration(PxArticulationCache& cache) const = 0;
289
290 /**
291 \brief determine the joint force
292 This is purely calculates the change in joint force due to change in the joint acceleration
293 This means gravity and joint velocity will be zero
294
295 \param[in] cache return joint force
296
297 @see commonInit
298 */
299 virtual void computeJointForce(PxArticulationCache& cache) const = 0;
300
301 /**
302 \brief compute the dense Jacobian for the entire articulation in world space
303 \param[out] cache sets cache.denseJacobian matrix. The matrix is indexed [nCols * row + column].
304 \param[out] nRows set to number of rows in matrix, which corresponds to the number of articulation links times 6.
305 \param[out] nCols set to number of columns in matrix, which corresponds to the number of joint DOFs, plus 6 in the case eFIX_BASE is false.
306
307 Note that this computes the dense representation of an inherently sparse matrix. Multiplication with this matrix maps
308 joint space velocities to 6DOF world space linear and angular velocities.
309 */
310 virtual void computeDenseJacobian(PxArticulationCache& cache, PxU32& nRows, PxU32& nCols) const = 0;
311
312
313 /**
314 \brief compute the coefficient matrix for contact force.
315 \param[out] cache returns the coefficient matrix. Each column is the joint force effected by a contact based on impulse strength 1
316 @see commonInit
317 */
318 virtual void computeCoefficientMatrix(PxArticulationCache& cache) const = 0;
319
320 /**
321 \brief compute the lambda value when the test impulse is 1
322 \param[in] initialState the initial state of the articulation system
323 \param[in] jointTorque M(q)*qddot + C(q,qdot) + g(q)
324 \param[in] maxIter maximum number of solver iterations to run. If the system converges, fewer iterations may be used.
325 \param[out] cache returns the coefficient matrix. Each column is the joint force effected by a contact based on impulse strength 1
326 @see commonInit
327 */
328 virtual bool computeLambda(PxArticulationCache& cache, PxArticulationCache& initialState, const PxReal* const jointTorque, const PxU32 maxIter) const = 0;
329
330 /**
331 \brief compute the joint-space inertia matrix
332 \param[in] cache articulation data
333
334 @see commonInit
335 */
336 virtual void computeGeneralizedMassMatrix(PxArticulationCache& cache) const = 0;
337
338 /**
339 \brief add loop joint to the articulation system for inverse dynamic
340 \param[in] joint required to add loop joint
341
342 @see commonInit
343 */
344 virtual void addLoopJoint(PxJoint* joint) = 0;
345
346 /**
347 \brief remove loop joint from the articulation system
348 \param[in] joint required to remove loop joint
349
350 @see commonInit
351 */
352 virtual void removeLoopJoint(PxJoint* joint) = 0;
353
354 /**
355 \brief returns the number of loop joints in the articulation
356 \return number of loop joints
357 */
358 virtual PxU32 getNbLoopJoints() const = 0;
359
360 /**
361 \brief returns the set of loop constraints in the articulation
362
363 \param[in] userBuffer buffer into which to write an array of constraints pointers
364 \param[in] bufferSize the size of the buffer. If this is not large enough to contain all the pointers to links,
365 only as many as will fit are written.
366 \param[in] startIndex Index of first link pointer to be retrieved
367
368 \return the number of links written into the buffer.
369
370 @see ArticulationLink
371 */
372 virtual PxU32 getLoopJoints(PxJoint** userBuffer, PxU32 bufferSize, PxU32 startIndex = 0) const = 0;
373
374 /**
375 \brief returns the required size of coeffient matrix in the articulation. The coefficient matrix is number of constraint(loop joints) by total dofs. Constraint Torque = transpose(K) * lambda(). Lambda is a vector of number of constraints
376 \return bite size of the coefficient matrix(nc * n)
377 */
378 virtual PxU32 getCoefficientMatrixSize() const = 0;
379
380 /**
381 \brief teleport root link to a new location
382 \param[in] pose the new location of the root link
383 \param[in] autowake wake up the articulation system
384
385 @see commonInit
386 */
387 virtual void teleportRootLink(const PxTransform& pose, bool autowake) = 0;
388
389
390 /**
391 \brief return the link velocity in world space with the associated low-level link index(getLinkIndex()).
392 \param[in] linkId low-level link index
393
394 @see getLinkIndex() in PxArticulationLink
395 */
396 virtual PxSpatialVelocity getLinkVelocity(const PxU32 linkId) = 0;
397
398
399 /**
400 \brief return the link acceleration in world space with the associated low-level link index(getLinkIndex())
401 \param[in] linkId low-level link index
402
403 @see getLinkIndex() in PxArticulationLink
404 */
405 virtual PxSpatialVelocity getLinkAcceleration(const PxU32 linkId) = 0;
406
407 protected:
408 PX_INLINE PxArticulationReducedCoordinate(PxType concreteType, PxBaseFlags baseFlags) : PxArticulationBase(concreteType, baseFlags) {}
409 PX_INLINE PxArticulationReducedCoordinate(PxBaseFlags baseFlags) : PxArticulationBase(baseFlags) {}
410 virtual ~PxArticulationReducedCoordinate() {}
411 };
412
413#if PX_VC
414#pragma warning(pop)
415#endif
416
417#if !PX_DOXYGEN
418} // namespace physx
419#endif
420
421 /** @} */
422#endif
423

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