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 DY_SOLVER_CONSTRAINT_1D_H
32#define DY_SOLVER_CONSTRAINT_1D_H
33
34#include "foundation/PxVec3.h"
35#include "PxvConfig.h"
36#include "DyArticulationUtils.h"
37#include "DySolverConstraintTypes.h"
38#include "DySolverBody.h"
39#include "PxConstraintDesc.h"
40#include "DySolverConstraintDesc.h"
41
42namespace physx
43{
44
45namespace Dy
46{
47
48// dsequeira: we should probably fork these structures for constraints and extended constraints,
49// since there's a few things that are used for one but not the other
50
51struct SolverConstraint1DHeader
52{
53 PxU8 type; // enum SolverConstraintType - must be first byte
54 PxU8 count; // count of following 1D constraints
55 PxU8 dominance;
56 PxU8 breakable; // indicate whether this constraint is breakable or not
57
58 PxReal linBreakImpulse;
59 PxReal angBreakImpulse;
60 PxReal invMass0D0;
61 PxVec3 body0WorldOffset;
62 PxReal invMass1D1;
63 PxReal linearInvMassScale0; // only used by articulations
64 PxReal angularInvMassScale0; // only used by articulations
65 PxReal linearInvMassScale1; // only used by articulations
66 PxReal angularInvMassScale1; // only used by articulations
67};
68
69PX_COMPILE_TIME_ASSERT(sizeof(SolverConstraint1DHeader) == 48);
70
71PX_ALIGN_PREFIX(16)
72struct SolverConstraint1D
73{
74public:
75 PxVec3 lin0; //!< linear velocity projection (body 0)
76 PxReal constant; //!< constraint constant term
77
78 PxVec3 lin1; //!< linear velocity projection (body 1)
79 PxReal unbiasedConstant; //!< constraint constant term without bias
80
81 PxVec3 ang0; //!< angular velocity projection (body 0)
82 PxReal velMultiplier; //!< constraint velocity multiplier
83
84 PxVec3 ang1; //!< angular velocity projection (body 1)
85 PxReal impulseMultiplier; //!< constraint impulse multiplier
86
87 PxVec3 ang0Writeback; //!< unscaled angular velocity projection (body 0)
88 PxU32 pad;
89
90 PxReal minImpulse; //!< Lower bound on impulse magnitude
91 PxReal maxImpulse; //!< Upper bound on impulse magnitude
92 PxReal appliedForce; //!< applied force to correct velocity+bias
93 PxU32 flags;
94} PX_ALIGN_SUFFIX(16);
95
96PX_COMPILE_TIME_ASSERT(sizeof(SolverConstraint1D) == 96);
97
98
99struct SolverConstraint1DExt : public SolverConstraint1D
100{
101public:
102 Cm::SpatialVectorV deltaVA;
103 Cm::SpatialVectorV deltaVB;
104};
105
106PX_COMPILE_TIME_ASSERT(sizeof(SolverConstraint1DExt) == 160);
107
108
109PX_FORCE_INLINE void init(SolverConstraint1DHeader& h,
110 PxU8 count,
111 bool isExtended,
112 const PxConstraintInvMassScale& ims)
113{
114 h.type = PxU8(isExtended ? DY_SC_TYPE_EXT_1D : DY_SC_TYPE_RB_1D);
115 h.count = count;
116 h.dominance = 0;
117 h.linearInvMassScale0 = ims.linear0;
118 h.angularInvMassScale0 = ims.angular0;
119 h.linearInvMassScale1 = -ims.linear1;
120 h.angularInvMassScale1 = -ims.angular1;
121}
122
123PX_FORCE_INLINE void init(SolverConstraint1D& c,
124 const PxVec3& _linear0, const PxVec3& _linear1,
125 const PxVec3& _angular0, const PxVec3& _angular1,
126 PxReal _minImpulse, PxReal _maxImpulse)
127{
128 PX_ASSERT(_linear0.isFinite());
129 PX_ASSERT(_linear1.isFinite());
130 c.lin0 = _linear0;
131 c.lin1 = _linear1;
132 c.ang0 = _angular0;
133 c.ang1 = _angular1;
134 c.minImpulse = _minImpulse;
135 c.maxImpulse = _maxImpulse;
136 c.flags = 0;
137 c.appliedForce = 0;
138}
139
140PX_FORCE_INLINE bool needsNormalVel(const Px1DConstraint &c)
141{
142 return c.flags & Px1DConstraintFlag::eRESTITUTION
143 || (c.flags & Px1DConstraintFlag::eSPRING && c.flags & Px1DConstraintFlag::eACCELERATION_SPRING);
144}
145
146PX_FORCE_INLINE void setSolverConstants(PxReal& constant,
147 PxReal& unbiasedConstant,
148 PxReal& velMultiplier,
149 PxReal& impulseMultiplier,
150 const Px1DConstraint& c,
151 PxReal normalVel,
152 PxReal unitResponse,
153 PxReal minRowResponse,
154 PxReal erp,
155 PxReal dt,
156 PxReal recipdt)
157{
158 PX_ASSERT(PxIsFinite(unitResponse));
159 PxReal recipResponse = unitResponse <= minRowResponse ? 0 : 1.0f/unitResponse;
160
161 PxReal geomError = c.geometricError * erp;
162
163 if(c.flags & Px1DConstraintFlag::eSPRING)
164 {
165 PxReal a = dt * dt * c.mods.spring.stiffness + dt * c.mods.spring.damping;
166 PxReal b = dt * (c.mods.spring.damping * c.velocityTarget - c.mods.spring.stiffness * geomError);
167
168 if(c.flags & Px1DConstraintFlag::eACCELERATION_SPRING)
169 {
170 PxReal x = 1.0f/(1.0f+a);
171 constant = unbiasedConstant = x * recipResponse * b;
172 velMultiplier = -x * recipResponse * a;
173 impulseMultiplier = 1.0f-x;
174 }
175 else
176 {
177 PxReal x = unitResponse == 0.f ? 0.f : 1.0f/(1.0f+a*unitResponse);
178 constant = unbiasedConstant = x * b;
179 velMultiplier = -x*a;
180 impulseMultiplier = 1.0f-x;
181 }
182 }
183 else
184 {
185 velMultiplier = -recipResponse;
186 impulseMultiplier = 1.0f;
187
188 if(c.flags & Px1DConstraintFlag::eRESTITUTION && -normalVel>c.mods.bounce.velocityThreshold)
189 {
190 unbiasedConstant = constant = recipResponse * c.mods.bounce.restitution*-normalVel;
191 }
192 else
193 {
194 // see usage of 'for internal use' in preprocessRows()
195 constant = recipResponse * (c.velocityTarget - geomError*recipdt);
196 unbiasedConstant = recipResponse * (c.velocityTarget - c.forInternalUse*recipdt);
197 }
198 }
199}
200
201}
202}
203
204#endif //DY_SOLVER_CONSTRAINT_1D_H
205

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