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_SCP_ARTICULATION_CORE
32#define PX_PHYSICS_SCP_ARTICULATION_CORE
33
34#include "ScActorCore.h"
35#include "DyArticulation.h"
36#include "DyFeatherstoneArticulation.h"
37
38namespace physx
39{
40
41class PxvArticulation;
42
43namespace IG
44{
45 class NodeIndex;
46}
47
48namespace Sc
49{
50 typedef Dy::FsData ArticulationDriveCache;
51
52 class ArticulationSim;
53 class BodyCore;
54 class BodySim;
55 class ArticulationJointCore;
56
57 class ArticulationCore
58 {
59 //= ATTENTION! =====================================================================================
60 // Changing the data layout of this class breaks the binary serialization format. See comments for
61 // PX_BINARY_SERIAL_VERSION. If a modification is required, please adjust the getBinaryMetaData
62 // function. If the modification is made on a custom branch, please change PX_BINARY_SERIAL_VERSION
63 // accordingly.
64 //==================================================================================================
65
66 //---------------------------------------------------------------------------------
67 // Construction, destruction & initialization
68 //---------------------------------------------------------------------------------
69
70// PX_SERIALIZATION
71 public:
72 ArticulationCore(const PxEMPTY) : mSim(NULL) {}
73 static void getBinaryMetaData(PxOutputStream& stream);
74//~PX_SERIALIZATION
75 ArticulationCore(bool reducedCoordinate);
76 ~ArticulationCore();
77
78 //---------------------------------------------------------------------------------
79 // External API
80 //---------------------------------------------------------------------------------
81 PX_FORCE_INLINE PxU32 getInternalDriveIterations() const { return mCore.internalDriveIterations; }
82 PX_FORCE_INLINE void setInternalDriveIterations(const PxU32 v) { mCore.internalDriveIterations = v; }
83
84 PX_FORCE_INLINE PxU32 getExternalDriveIterations() const { return mCore.externalDriveIterations; }
85 PX_FORCE_INLINE void setExternalDriveIterations(const PxU32 v) { mCore.externalDriveIterations = v; }
86
87 PX_FORCE_INLINE PxU32 getMaxProjectionIterations() const { return mCore.maxProjectionIterations; }
88 PX_FORCE_INLINE void setMaxProjectionIterations(const PxU32 v) { mCore.maxProjectionIterations = v; }
89
90 PX_FORCE_INLINE PxReal getSeparationTolerance() const { return mCore.separationTolerance; }
91 PX_FORCE_INLINE void setSeparationTolerance(const PxReal v) { mCore.separationTolerance = v; }
92
93 PX_FORCE_INLINE PxReal getSleepThreshold() const { return mCore.sleepThreshold; }
94 PX_FORCE_INLINE void setSleepThreshold(const PxReal v) { mCore.sleepThreshold = v; }
95
96 PX_FORCE_INLINE PxReal getFreezeThreshold() const { return mCore.freezeThreshold; }
97 PX_FORCE_INLINE void setFreezeThreshold(const PxReal v) { mCore.freezeThreshold = v; }
98
99 PX_FORCE_INLINE PxU16 getSolverIterationCounts() const { return mCore.solverIterationCounts; }
100 PX_FORCE_INLINE void setSolverIterationCounts(PxU16 c) { mCore.solverIterationCounts = c; }
101
102 PX_FORCE_INLINE PxReal getWakeCounter() const { return mCore.wakeCounter; }
103 PX_FORCE_INLINE void setWakeCounterInternal(const PxReal v) { mCore.wakeCounter = v; }
104 void setWakeCounter(const PxReal v);
105
106 bool isSleeping() const;
107 void wakeUp(PxReal wakeCounter);
108 void putToSleep();
109
110 PxArticulationBase* getPxArticulationBase();
111 const PxArticulationBase* getPxArticulationBase() const;
112
113 //---------------------------------------------------------------------------------
114 // Drive Cache API
115 //---------------------------------------------------------------------------------
116 ArticulationDriveCache* createDriveCache(PxReal compliance,
117 PxU32 driveIterations) const;
118
119 void updateDriveCache(ArticulationDriveCache& cache,
120 PxReal compliance,
121 PxU32 driveIterations) const;
122
123 void releaseDriveCache(ArticulationDriveCache& cache) const;
124
125 PxU32 getCacheLinkCount(const ArticulationDriveCache& cache) const;
126
127 void applyImpulse(BodyCore& link,
128 const ArticulationDriveCache& driveCache,
129 const PxVec3& force,
130 const PxVec3& torque);
131
132 void computeImpulseResponse(BodyCore& link,
133 PxVec3& linearResponse,
134 PxVec3& angularResponse,
135 const ArticulationDriveCache& driveCache,
136 const PxVec3& force,
137 const PxVec3& torque) const;
138
139 //---------------------------------------------------------------------------------
140 // external reduced coordinate API
141 //---------------------------------------------------------------------------------
142 void setArticulationFlags(PxArticulationFlags flags);
143 PxArticulationFlags getArticulationFlags() const { return mCore.flags; }
144
145 PxU32 getDofs() const;
146
147 PxArticulationCache* createCache() const;
148
149 PxU32 getCacheDataSize() const;
150
151 void zeroCache(PxArticulationCache& cache) const;
152
153 void applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag)const;
154
155 void copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const;
156
157 void releaseCache(PxArticulationCache& cache) const;
158
159 void packJointData(const PxReal* maximum, PxReal* reduced) const;
160
161 void unpackJointData(const PxReal* reduced, PxReal* maximum) const;
162
163 void commonInit() const;
164
165 void computeGeneralizedGravityForce(PxArticulationCache& cache) const;
166
167 void computeCoriolisAndCentrifugalForce(PxArticulationCache& cache) const;
168
169 void computeGeneralizedExternalForce(PxArticulationCache& cache) const;
170
171 void computeJointAcceleration(PxArticulationCache& cache) const;
172
173 void computeJointForce(PxArticulationCache& cache) const;
174
175
176 void computeDenseJacobian(PxArticulationCache& cache, PxU32& nRows, PxU32& nCols) const;
177
178 void computeCoefficientMatrix(PxArticulationCache& cache) const;
179
180 bool computeLambda(PxArticulationCache& cache, PxArticulationCache& rollBackCache, const PxReal* const jointTorque, const PxVec3 gravity, const PxU32 maxIter) const;
181
182 void computeGeneralizedMassMatrix(PxArticulationCache& cache) const;
183
184 PxU32 getCoefficientMatrixSize() const;
185
186 PxSpatialVelocity getLinkVelocity(const PxU32 linkId) const;
187
188 PxSpatialVelocity getLinkAcceleration(const PxU32 linkId) const;
189
190 //---------------------------------------------------------------------------------
191 // Internal API
192 //---------------------------------------------------------------------------------
193 public:
194 PX_FORCE_INLINE void setSim(ArticulationSim* sim)
195 {
196 PX_ASSERT((sim==0) ^ (mSim == 0));
197 mSim = sim;
198 }
199 PX_FORCE_INLINE ArticulationSim* getSim() const { return mSim; }
200
201 PX_FORCE_INLINE const Dy::ArticulationCore& getCore() { return mCore; }
202
203 static PX_FORCE_INLINE ArticulationCore& getArticulationCore(ArticulationCore& core)
204 {
205 const size_t offset = PX_OFFSET_OF(ArticulationCore, mCore);
206 return *reinterpret_cast<ArticulationCore*>(reinterpret_cast<PxU8*>(&core) - offset);
207 }
208
209 PX_INLINE bool isReducedCoordinate() const { return mIsReducedCoordinate; }
210
211 IG::NodeIndex getIslandNodeIndex() const;
212
213 void setGlobalPose();
214
215 void setDirty(const bool dirty);
216
217 private:
218 ArticulationSim* mSim;
219 Dy::ArticulationCore mCore;
220 bool mIsReducedCoordinate;
221 };
222
223} // namespace Sc
224
225}
226
227#endif
228

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