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_FEATHERSTONE_ARTICULATION_JOINTCORE_H
32#define PXD_FEATHERSTONE_ARTICULATION_JOINTCORE_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 "DyVArticulation.h"
41#include "DyFeatherstoneArticulationUtils.h"
42#include "DyArticulationJointCore.h"
43#include <stdio.h>
44
45namespace physx
46{
47 namespace Dy
48 {
49
50
51 class PX_ALIGN_PREFIX(16) ArticulationJointCoreData
52 {
53 public:
54
55 ArticulationJointCoreData() : jointOffset(0xffffffff), dofInternalConstraintMask(0)
56 {
57 }
58
59
60 PX_CUDA_CALLABLE PX_FORCE_INLINE void computeMotionMatrix(ArticulationJointCoreBase* joint,
61 SpatialSubspaceMatrix& motionMatrix)
62 {
63 const PxVec3 childOffset = -joint->childPose.p;
64
65 //transpose(Tc)*S = 0
66 //transpose(Ta)*S = 1
67 switch (joint->jointType)
68 {
69 case PxArticulationJointType::ePRISMATIC:
70 {
71 const Cm::UnAlignedSpatialVector& jJointAxis = jointAxis[0];
72 const PxVec3 u = (joint->childPose.rotate(input: jJointAxis.bottom)).getNormalized();
73
74 motionMatrix.setNumColumns(1);
75 motionMatrix.setColumn(index: 0, top: PxVec3(0.f), bottom: u);
76
77 PX_ASSERT(dof == 1);
78
79 break;
80 }
81 case PxArticulationJointType::eREVOLUTE:
82 {
83 const Cm::UnAlignedSpatialVector& jJointAxis = jointAxis[0];
84 const PxVec3 u = (joint->childPose.rotate(input: jJointAxis.top)).getNormalized();
85 const PxVec3 uXd = u.cross(v: childOffset);
86
87 motionMatrix.setNumColumns(1);
88 motionMatrix.setColumn(index: 0, top: u, bottom: uXd);
89
90 break;
91 }
92 case PxArticulationJointType::eSPHERICAL:
93 {
94 motionMatrix.setNumColumns(dof);
95
96 for (PxU32 ind = 0; ind <dof; ++ind)
97 {
98 const Cm::UnAlignedSpatialVector& jJointAxis = jointAxis[ind];
99 const PxVec3 u = (joint->childPose.rotate(input: jJointAxis.top)).getNormalized();
100
101 const PxVec3 uXd = u.cross(v: childOffset);
102 motionMatrix.setColumn(index: ind, top: u, bottom: uXd);
103 }
104
105 break;
106 }
107 case PxArticulationJointType::eFIX:
108 {
109 motionMatrix.setNumColumns(0);
110
111 PX_ASSERT(dof == 0);
112 break;
113 }
114 default:
115 break;
116 }
117 }
118
119 PX_CUDA_CALLABLE PX_FORCE_INLINE PxU8 computeJointDofs(ArticulationJointCoreBase* joint) const
120 {
121 PxU8 tDof = 0;
122
123 for (PxU32 i = 0; i < DY_MAX_DOF; ++i)
124 {
125 if (joint->motion[i] != PxArticulationMotion::eLOCKED)
126 {
127 tDof++;
128 }
129 }
130
131 return tDof;
132 }
133
134 PX_CUDA_CALLABLE PX_FORCE_INLINE void computeJointDof(ArticulationJointCoreBase* joint, const bool forceRecompute)
135 {
136 if (joint->dirtyFlag & ArticulationJointCoreDirtyFlag::eMOTION || forceRecompute)
137 {
138
139 dof = 0;
140 lockedAxes = 0;
141 limitedAxes = 0;
142
143 //KS - no need to zero memory here.
144 //PxMemZero(jointAxis, sizeof(jointAxis));
145
146 for (PxU8 i = 0; i < DY_MAX_DOF; ++i)
147 {
148 if (joint->motion[i] != PxArticulationMotion::eLOCKED)
149 {
150 //axis is in the local space of joint
151 jointAxis[dof][i] = 1.f;
152
153 if (joint->motion[i] == PxArticulationMotion::eLIMITED)
154 {
155 limitedAxes++;
156 }
157
158 joint->dofIds[dof++] = i;
159 }
160 }
161
162 lockedAxes = 0;
163
164#if 1
165 //Spherical joints treat locked axes as free axes with a constraint. This produces better
166 //results for spherical joints with 2 dofs free, where keeping the 3rd axis locked can lead to
167 //an over-consrtained behaviour that is undesirable. However, the drawback is that there will be
168 //some drift and error on the joint axes
169 if (joint->jointType == PxArticulationJointType::eSPHERICAL && dof == 2)
170 {
171 for (PxU32 i = 0; i < PxArticulationAxis::eX; ++i)
172 {
173 if (joint->motion[i] == PxArticulationMotion::eLOCKED)
174 {
175 //axis is in the local space of joint
176 jointAxis[dof][i] = 1.f;
177 joint->dofIds[dof++] = PxU8(i);
178 lockedAxes++;
179 }
180 }
181 }
182#endif
183
184 joint->dirtyFlag &= (~ArticulationJointCoreDirtyFlag::eMOTION);
185 }
186
187 }
188
189 //in the joint space
190 Cm::UnAlignedSpatialVector jointAxis[3]; //72
191 //this is the dof offset for the joint in the cache
192 PxU32 jointOffset; //76
193 //degree of freedom
194 PxU8 dof; //77
195 PxU8 limitedAxes; //78
196 PxU8 dofInternalConstraintMask; //79
197 PxU8 lockedAxes; //80
198
199 } PX_ALIGN_SUFFIX(16);
200
201 struct PX_ALIGN_PREFIX(16) ArticulationJointTargetData
202 {
203 PxReal targetJointVelocity[3]; //12
204 PxReal targetJointPosition[3]; //24
205 Cm::UnAlignedSpatialVector worldJointAxis[3]; //96
206 //PxU32 pad[2];
207
208
209
210
211 ArticulationJointTargetData()
212 {
213 for (PxU32 i = 0; i < 3; ++i)
214 {
215 targetJointPosition[i] = 0.f;
216 targetJointVelocity[i] = 0.f;
217 }
218 }
219
220
221 PX_CUDA_CALLABLE PX_FORCE_INLINE void setJointVelocityDrive(ArticulationJointCoreBase* joint)
222 {
223 if (joint->dirtyFlag & ArticulationJointCoreDirtyFlag::eTARGETVELOCITY)
224 {
225 PxU32 count = 0;
226 for (PxU32 i = 0; i < DY_MAX_DOF; ++i)
227 {
228 if (joint->motion[i] != PxArticulationMotion::eLOCKED)
229 {
230 targetJointVelocity[count] = joint->targetV[i];
231 count++;
232 }
233 }
234 joint->dirtyFlag &= ~ArticulationJointCoreDirtyFlag::eTARGETVELOCITY;
235 }
236 }
237
238 PX_CUDA_CALLABLE PX_FORCE_INLINE void setJointPoseDrive(ArticulationJointCoreBase* joint)
239 {
240 if (joint->dirtyFlag & ArticulationJointCoreDirtyFlag::eTARGETPOSE)
241 {
242 PxU32 count = 0;
243 for (PxU32 i = 0; i < DY_MAX_DOF; ++i)
244 {
245 if (joint->motion[i] != PxArticulationMotion::eLOCKED)
246 {
247 targetJointPosition[count] = joint->targetP[i];
248 count++;
249 }
250 }
251
252 joint->dirtyFlag &= ~ArticulationJointCoreDirtyFlag::eTARGETPOSE;
253 }
254 }
255
256 } PX_ALIGN_SUFFIX(16);
257
258 }//namespace Dy
259}
260
261#endif
262

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