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 PXD_ARTICULATION_H
32#define PXD_ARTICULATION_H
33
34#include "foundation/PxVec3.h"
35#include "foundation/PxQuat.h"
36#include "foundation/PxTransform.h"
37#include "PsVecMath.h"
38#include "CmUtils.h"
39#include "CmSpatialVector.h"
40#include "PxArticulationJoint.h"
41#include "DyVArticulation.h"
42
43namespace physx
44{
45 class PxConstraintAllocator;
46 class PxcConstraintBlockStream;
47 struct PxSolverConstraintDesc;
48 class PxsConstraintBlockManager;
49
50#define DY_DEBUG_ARTICULATION 0
51
52namespace Dy
53{
54 struct ArticulationJointTransforms;
55 struct FsInertia;
56 struct FsData;
57
58#if PX_VC
59 #pragma warning(push)
60 #pragma warning( disable : 4324 ) // Padding was added at the end of a structure because of a __declspec(align) value.
61#endif
62
63PX_ALIGN_PREFIX(64)
64
65class Articulation : public ArticulationV
66{
67public:
68 // public interface
69
70 Articulation(void*);
71 ~Articulation();
72
73 virtual bool resize(const PxU32 linkCount);
74
75 virtual void onUpdateSolverDesc()
76 {
77 PxMemZero(dest: mExternalLoads.begin(), count: sizeof(Ps::aos::Mat33V) * mExternalLoads.size());
78 PxMemZero(dest: mInternalLoads.begin(), count: sizeof(Ps::aos::Mat33V) * mExternalLoads.size());
79 }
80
81 FsData* getFsDataPtr() const { return reinterpret_cast<FsData *>(mFsDataBytes.begin()); }
82 //void setFsDataPtr(FsData* data) { mFsData = data; }
83
84 // get data sizes for allocation at higher levels
85 virtual void getDataSizes(PxU32 linkCount, PxU32& solverDataSize, PxU32& totalSize, PxU32& scratchSize);
86
87 virtual void getImpulseResponse(
88 PxU32 linkID,
89 Cm::SpatialVectorF* Z,
90 const Cm::SpatialVector& impulse,
91 Cm::SpatialVector& deltaV) const;
92
93 virtual void getImpulseResponse(
94 PxU32 linkID,
95 Cm::SpatialVectorV* Z,
96 const Cm::SpatialVectorV& impulse,
97 Cm::SpatialVectorV& deltaV) const;
98
99 virtual void getImpulseSelfResponse(
100 PxU32 linkID0,
101 PxU32 linkID1,
102 Cm::SpatialVectorF* Z,
103 const Cm::SpatialVector& impulse0,
104 const Cm::SpatialVector& impulse1,
105 Cm::SpatialVector& deltaV0,
106 Cm::SpatialVector& deltaV1) const;
107
108 virtual Cm::SpatialVectorV getLinkVelocity(const PxU32 linkID) const;
109
110 virtual Cm::SpatialVectorV getLinkMotionVector(const PxU32 linkID) const;
111
112 //this is called by island gen to determine whether the articulation should be awake or sleep
113 virtual Cm::SpatialVector getMotionVelocity(const PxU32 linkID) const;
114
115 virtual Cm::SpatialVector getMotionAcceleration(const PxU32 linkID) const;
116
117 virtual void fillIndexedManager(const PxU32 linkId, Dy::ArticulationLinkHandle& handle, PxU8& indexType);
118
119 virtual PxReal getLinkMaxPenBias(const PxU32 linkID) const;
120
121
122 static PxU32 computeUnconstrainedVelocities(
123 const ArticulationSolverDesc& desc,
124 PxReal dt,
125 PxConstraintAllocator& allocator,
126 PxSolverConstraintDesc* constraintDesc,
127 PxU32& acCount,
128 const PxVec3& gravity, PxU64 contextID,
129 Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
130
131 static void computeUnconstrainedVelocitiesTGS(
132 const ArticulationSolverDesc& desc,
133 PxReal dt, const PxVec3& gravity,
134 PxU64 contextID, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
135
136 static PxU32 setupSolverConstraintsTGS(const ArticulationSolverDesc& articDesc,
137 PxcConstraintBlockStream& stream,
138 PxSolverConstraintDesc* constraintDesc,
139 PxReal dt,
140 PxReal invDt,
141 PxReal totalDt,
142 PxU32& acCount,
143 PxsConstraintBlockManager& constraintBlockManager,
144 Cm::SpatialVectorF* /*Z*/);
145
146 static void saveVelocity(const ArticulationSolverDesc& d, Cm::SpatialVectorF* deltaV);
147
148 static void saveVelocityTGS(const ArticulationSolverDesc& d, PxReal invDtF32);
149
150 static void updateBodies(const ArticulationSolverDesc& desc, PxReal dt);
151
152 static void recordDeltaMotion(const ArticulationSolverDesc &desc, const PxReal dt, Cm::SpatialVectorF* deltaV, PxReal);
153
154 static void deltaMotionToMotionVelocity(const ArticulationSolverDesc& desc, PxReal invDt);
155
156 virtual void pxcFsApplyImpulse(PxU32 linkID, Ps::aos::Vec3V linear,
157 Ps::aos::Vec3V angular, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
158
159 virtual void pxcFsApplyImpulses(PxU32 linkID, const Ps::aos::Vec3V& linear,
160 const Ps::aos::Vec3V& angular, PxU32 linkID2, const Ps::aos::Vec3V& linear2,
161 const Ps::aos::Vec3V& angular2, Cm::SpatialVectorF* Z, Cm::SpatialVectorF* deltaV);
162
163 virtual void solveInternalConstraints(const PxReal dt, const PxReal invDt, Cm::SpatialVectorF* impulses, Cm::SpatialVectorF* DeltaV,
164 bool velIteration, bool isTGS, const PxReal elapsedTime);
165
166 virtual void writebackInternalConstraints(bool /*isTGS*/) {}
167
168 virtual void concludeInternalConstraints(bool /*isTGS*/) {}
169
170 virtual Cm::SpatialVectorV pxcFsGetVelocity(PxU32 linkID);
171 virtual void pxcFsGetVelocities(PxU32 linkID, PxU32 linkID1, Cm::SpatialVectorV& v0, Cm::SpatialVectorV& v1);
172
173 virtual Cm::SpatialVectorV pxcFsGetVelocityTGS(PxU32 linkID) { return Articulation::pxcFsGetVelocity(linkID); }
174
175 virtual const PxTransform& getCurrentTransform(PxU32 linkID)const
176 {
177 return mPose[linkID];
178 }
179
180 virtual const PxQuat& getDeltaQ(PxU32 linkID) const
181 {
182 return mDeltaQ[linkID];
183 }
184
185 static void prepareDataBlock(FsData& fsData,
186 const ArticulationLink* links,
187 PxU16 linkCount,
188 PxTransform* poses,
189 PxQuat* deltaQ,
190 FsInertia* baseInertia,
191 ArticulationJointTransforms* jointTransforms,
192 PxU32 expectedSize);
193
194 static void prepareFsData(FsData& fsData,
195 const ArticulationLink* links);
196
197 static PxReal getResistance(PxReal compliance);
198
199 static PxU32 getFsDataSize(PxU32 linkCount);
200
201 static PxU32 getLtbDataSize(PxU32 linkCount);
202
203 static void setInertia(FsInertia& inertia,
204 const PxsBodyCore& body,
205 const PxTransform& pose);
206
207 static void setJointTransforms(ArticulationJointTransforms& transforms,
208 const PxTransform& parentPose,
209 const PxTransform& childPose,
210 const ArticulationJointCore& joint);
211
212 static void applyImpulses(const FsData& matrix,
213 Cm::SpatialVectorV* Z,
214 Cm::SpatialVectorV* V);
215
216private:
217
218#if DY_DEBUG_ARTICULATION
219 // debug quantities
220
221 Cm::SpatialVector computeMomentum(const FsInertia *inertia) const;
222 void computeResiduals(const Cm::SpatialVector *,
223 const ArticulationJointTransforms* jointTransforms,
224 bool dump = false) const;
225 void checkLimits() const;
226#endif
227
228 //PX_FORCE_INLINE Cm::SpatialVectorV* getVelocity(FsData& matrix);
229
230 void computeUnconstrainedVelocitiesInternal(const ArticulationSolverDesc& desc,
231 PxReal dt,
232 const PxVec3& gravity, PxU64 contextID,
233 FsInertia* PX_RESTRICT baseInertia,
234 ArticulationJointTransforms* PX_RESTRICT jointTransforms,
235 PxcFsScratchAllocator& allocator);
236
237 void prepareLtbMatrix(FsData& fsData,
238 const FsInertia* baseInertia,
239 const PxTransform* poses,
240 const ArticulationJointTransforms* jointTransforms,
241 PxReal recipDt);
242
243 void computeJointDrives(FsData& fsData,
244 Ps::aos::Vec3V* drives,
245 const ArticulationLink* links,
246 const PxTransform* poses,
247 const ArticulationJointTransforms* transforms,
248 const Ps::aos::Mat33V* loads,
249 PxReal dt);
250
251 mutable Ps::Array<char> mFsDataBytes; // drive cache creation (which is const) can force a resize
252
253 // persistent state of the articulation for warm-starting joint load computation
254 Ps::Array<Ps::aos::Mat33V> mInternalLoads;
255 Ps::Array<Ps::aos::Mat33V> mExternalLoads;
256 Ps::Array<char> mScratchMemory; // drive cache creation (which is const) can force a resize
257 Ps::Array<PxTransform> mPose;
258 Ps::Array<PxQuat> mDeltaQ;
259 Ps::Array<Cm::SpatialVectorV> mMotionVelocity; // saved here in solver to communicate back to island management/sleeping
260
261} PX_ALIGN_SUFFIX(64);
262
263#if PX_VC
264 #pragma warning(pop)
265#endif
266
267class PxvArticulationDriveCache
268{
269public:
270 // drive cache stuff
271 static void initialize(
272 FsData &cache,
273 PxU16 linkCount,
274 const ArticulationLink* links,
275 PxReal compliance,
276 PxU32 iterations,
277 char* scratchMemory,
278 PxU32 scratchMemorySize);
279
280 static PxU32 getLinkCount(const FsData& cache);
281
282 static void applyImpulses(const FsData& cache,
283 Cm::SpatialVectorV* Z,
284 Cm::SpatialVectorV* V);
285
286 static void getImpulseResponse(const FsData& cache,
287 PxU32 linkID,
288 const Cm::SpatialVectorV& impulse,
289 Cm::SpatialVectorV& deltaV);
290};
291
292void PxvRegisterArticulations();
293
294}
295
296}
297
298#endif
299

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