aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs880
1 files changed, 440 insertions, 440 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
index 479c863..f1e0b1e 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
@@ -1,440 +1,440 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software 14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space 30 /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
31 /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked' 31 /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
32 /// Work in progress (is still a Hinge actually) 32 /// Work in progress (is still a Hinge actually)
33 /// </summary> 33 /// </summary>
34 public class Generic6DofConstraint : TypedConstraint 34 public class Generic6DofConstraint : TypedConstraint
35 { 35 {
36 private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f }; 36 private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f };
37 private static readonly int[] _axisA = { 1, 0, 0 }; 37 private static readonly int[] _axisA = { 1, 0, 0 };
38 private static readonly int[] _axisB = { 2, 2, 1 }; 38 private static readonly int[] _axisB = { 2, 2, 1 };
39 39
40 private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints 40 private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints
41 private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints 41 private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints
42 42
43 private Matrix _frameInA; // the constraint space w.r.t body A 43 private Matrix _frameInA; // the constraint space w.r.t body A
44 private Matrix _frameInB; // the constraint space w.r.t body B 44 private Matrix _frameInB; // the constraint space w.r.t body B
45 45
46 private float[] _lowerLimit = new float[6]; // the constraint lower limits 46 private float[] _lowerLimit = new float[6]; // the constraint lower limits
47 private float[] _upperLimit = new float[6]; // the constraint upper limits 47 private float[] _upperLimit = new float[6]; // the constraint upper limits
48 48
49 private float[] _accumulatedImpulse = new float[6]; 49 private float[] _accumulatedImpulse = new float[6];
50 50
51 public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB) 51 public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB)
52 : base(rbA, rbB) 52 : base(rbA, rbB)
53 { 53 {
54 _frameInA = frameInA; 54 _frameInA = frameInA;
55 _frameInB = frameInB; 55 _frameInB = frameInB;
56 //free means upper < lower, 56 //free means upper < lower,
57 //locked means upper == lower 57 //locked means upper == lower
58 //limited means upper > lower 58 //limited means upper > lower
59 //so start all locked 59 //so start all locked
60 for (int i = 0; i < 6; ++i) 60 for (int i = 0; i < 6; ++i)
61 { 61 {
62 _lowerLimit[i] = 0.0f; 62 _lowerLimit[i] = 0.0f;
63 _upperLimit[i] = 0.0f; 63 _upperLimit[i] = 0.0f;
64 _accumulatedImpulse[i] = 0.0f; 64 _accumulatedImpulse[i] = 0.0f;
65 } 65 }
66 } 66 }
67 67
68 public Generic6DofConstraint() { } 68 public Generic6DofConstraint() { }
69 69
70 public void UpdateRHS(float timeStep) { } 70 public void UpdateRHS(float timeStep) { }
71 71
72 public float ComputeAngle(int axis) 72 public float ComputeAngle(int axis)
73 { 73 {
74 float angle = 0; 74 float angle = 0;
75 75
76 switch (axis) 76 switch (axis)
77 { 77 {
78 case 0: 78 case 0:
79 { 79 {
80 Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform); 80 Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform);
81 Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); 81 Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);
82 Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); 82 Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);
83 83
84 float s = Vector3.Dot(v1, w2); 84 float s = Vector3.Dot(v1, w2);
85 float c = Vector3.Dot(v1, v2); 85 float c = Vector3.Dot(v1, v2);
86 86
87 angle = (float)Math.Atan2(s, c); 87 angle = (float)Math.Atan2(s, c);
88 break; 88 break;
89 } 89 }
90 case 1: 90 case 1:
91 { 91 {
92 Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform); 92 Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform);
93 Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform); 93 Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);
94 Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); 94 Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);
95 95
96 float s = Vector3.Dot(w1, u2); 96 float s = Vector3.Dot(w1, u2);
97 float c = Vector3.Dot(w1, w2); 97 float c = Vector3.Dot(w1, w2);
98 98
99 angle = (float)Math.Atan2(s, c); 99 angle = (float)Math.Atan2(s, c);
100 break; 100 break;
101 } 101 }
102 case 2: 102 case 2:
103 { 103 {
104 Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform); 104 Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform);
105 Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform); 105 Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);
106 Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform); 106 Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);
107 107
108 float s = Vector3.Dot(u1, v2); 108 float s = Vector3.Dot(u1, v2);
109 float c = Vector3.Dot(u1, u2); 109 float c = Vector3.Dot(u1, u2);
110 110
111 angle = (float)Math.Atan2(s, c); 111 angle = (float)Math.Atan2(s, c);
112 break; 112 break;
113 } 113 }
114 default: BulletDebug.Assert(false); break; 114 default: BulletDebug.Assert(false); break;
115 } 115 }
116 116
117 return angle; 117 return angle;
118 } 118 }
119 119
120 public void SetLinearLowerLimit(Vector3 linearLower) 120 public void SetLinearLowerLimit(Vector3 linearLower)
121 { 121 {
122 _lowerLimit[0] = linearLower.X; 122 _lowerLimit[0] = linearLower.X;
123 _lowerLimit[1] = linearLower.Y; 123 _lowerLimit[1] = linearLower.Y;
124 _lowerLimit[2] = linearLower.Z; 124 _lowerLimit[2] = linearLower.Z;
125 } 125 }
126 126
127 public void SetLinearUpperLimit(Vector3 linearUpper) 127 public void SetLinearUpperLimit(Vector3 linearUpper)
128 { 128 {
129 _upperLimit[0] = linearUpper.X; 129 _upperLimit[0] = linearUpper.X;
130 _upperLimit[1] = linearUpper.Y; 130 _upperLimit[1] = linearUpper.Y;
131 _upperLimit[2] = linearUpper.Z; 131 _upperLimit[2] = linearUpper.Z;
132 } 132 }
133 133
134 public void SetAngularLowerLimit(Vector3 angularLower) 134 public void SetAngularLowerLimit(Vector3 angularLower)
135 { 135 {
136 _lowerLimit[3] = angularLower.X; 136 _lowerLimit[3] = angularLower.X;
137 _lowerLimit[4] = angularLower.Y; 137 _lowerLimit[4] = angularLower.Y;
138 _lowerLimit[5] = angularLower.Z; 138 _lowerLimit[5] = angularLower.Z;
139 } 139 }
140 140
141 public void SetAngularUpperLimit(Vector3 angularUpper) 141 public void SetAngularUpperLimit(Vector3 angularUpper)
142 { 142 {
143 _upperLimit[3] = angularUpper.X; 143 _upperLimit[3] = angularUpper.X;
144 _upperLimit[4] = angularUpper.Y; 144 _upperLimit[4] = angularUpper.Y;
145 _upperLimit[5] = angularUpper.Z; 145 _upperLimit[5] = angularUpper.Z;
146 } 146 }
147 147
148 //first 3 are linear, next 3 are angular 148 //first 3 are linear, next 3 are angular
149 public void SetLimit(int axis, float lo, float hi) 149 public void SetLimit(int axis, float lo, float hi)
150 { 150 {
151 _lowerLimit[axis] = lo; 151 _lowerLimit[axis] = lo;
152 _upperLimit[axis] = hi; 152 _upperLimit[axis] = hi;
153 } 153 }
154 154
155 //free means upper < lower, 155 //free means upper < lower,
156 //locked means upper == lower 156 //locked means upper == lower
157 //limited means upper > lower 157 //limited means upper > lower
158 //limitIndex: first 3 are linear, next 3 are angular 158 //limitIndex: first 3 are linear, next 3 are angular
159 public bool IsLimited(int limitIndex) 159 public bool IsLimited(int limitIndex)
160 { 160 {
161 return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]); 161 return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]);
162 } 162 }
163 163
164 public override void BuildJacobian() 164 public override void BuildJacobian()
165 { 165 {
166 Vector3 localNormalInA = new Vector3(0, 0, 0); 166 Vector3 localNormalInA = new Vector3(0, 0, 0);
167 167
168 Vector3 pivotInA = _frameInA.Translation; 168 Vector3 pivotInA = _frameInA.Translation;
169 Vector3 pivotInB = _frameInB.Translation; 169 Vector3 pivotInB = _frameInB.Translation;
170 170
171 Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); 171 Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
172 Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); 172 Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);
173 173
174 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; 174 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
175 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; 175 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
176 176
177 //linear part 177 //linear part
178 for (int i = 0; i < 3; i++) 178 for (int i = 0; i < 3; i++)
179 { 179 {
180 if (IsLimited(i)) 180 if (IsLimited(i))
181 { 181 {
182 if (i == 0) 182 if (i == 0)
183 localNormalInA = new Vector3(1, 0, 0); 183 localNormalInA = new Vector3(1, 0, 0);
184 else if (i == 1) 184 else if (i == 1)
185 localNormalInA = new Vector3(0, 1, 0); 185 localNormalInA = new Vector3(0, 1, 0);
186 else 186 else
187 localNormalInA = new Vector3(0, 0, 1); 187 localNormalInA = new Vector3(0, 0, 1);
188 188
189 Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); 189 Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);
190 190
191 // Create linear atom 191 // Create linear atom
192 _jacLinear[i] = new JacobianEntry( 192 _jacLinear[i] = new JacobianEntry(
193 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), 193 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
194 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), 194 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
195 MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, 195 MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
196 MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, 196 MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
197 normalWorld, 197 normalWorld,
198 RigidBodyA.InvInertiaDiagLocal, 198 RigidBodyA.InvInertiaDiagLocal,
199 RigidBodyA.InverseMass, 199 RigidBodyA.InverseMass,
200 RigidBodyB.InvInertiaDiagLocal, 200 RigidBodyB.InvInertiaDiagLocal,
201 RigidBodyB.InverseMass); 201 RigidBodyB.InverseMass);
202 202
203 //optionally disable warmstarting 203 //optionally disable warmstarting
204 _accumulatedImpulse[i] = 0f; 204 _accumulatedImpulse[i] = 0f;
205 205
206 // Apply accumulated impulse 206 // Apply accumulated impulse
207 Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld; 207 Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld;
208 208
209 RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); 209 RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
210 RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); 210 RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
211 } 211 }
212 } 212 }
213 213
214 // angular part 214 // angular part
215 for (int i = 0; i < 3; i++) 215 for (int i = 0; i < 3; i++)
216 { 216 {
217 if (IsLimited(i + 3)) 217 if (IsLimited(i + 3))
218 { 218 {
219 Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); 219 Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);
220 Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); 220 Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);
221 221
222 Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB); 222 Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB);
223 223
224 // Create angular atom 224 // Create angular atom
225 _jacAng[i] = new JacobianEntry(axis, 225 _jacAng[i] = new JacobianEntry(axis,
226 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), 226 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
227 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), 227 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
228 RigidBodyA.InvInertiaDiagLocal, 228 RigidBodyA.InvInertiaDiagLocal,
229 RigidBodyB.InvInertiaDiagLocal); 229 RigidBodyB.InvInertiaDiagLocal);
230 230
231 _accumulatedImpulse[i + 3] = 0f; 231 _accumulatedImpulse[i + 3] = 0f;
232 232
233 // Apply accumulated impulse 233 // Apply accumulated impulse
234 Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis; 234 Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis;
235 235
236 RigidBodyA.ApplyTorqueImpulse(impulse_vector); 236 RigidBodyA.ApplyTorqueImpulse(impulse_vector);
237 RigidBodyB.ApplyTorqueImpulse(-impulse_vector); 237 RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
238 } 238 }
239 } 239 }
240 } 240 }
241 241
242 public override void SolveConstraint(float timeStep) 242 public override void SolveConstraint(float timeStep)
243 { 243 {
244 float tau = 0.1f; 244 float tau = 0.1f;
245 float damping = 1.0f; 245 float damping = 1.0f;
246 246
247 Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform); 247 Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
248 Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform); 248 Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);
249 249
250 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; 250 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
251 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; 251 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
252 252
253 Vector3 localNormalInA = new Vector3(); 253 Vector3 localNormalInA = new Vector3();
254 254
255 // linear 255 // linear
256 for (int i = 0; i < 3; i++) 256 for (int i = 0; i < 3; i++)
257 { 257 {
258 if (IsLimited(i)) 258 if (IsLimited(i))
259 { 259 {
260 Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); 260 Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
261 Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); 261 Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));
262 262
263 if (i == 0) 263 if (i == 0)
264 localNormalInA = new Vector3(1, 0, 0); 264 localNormalInA = new Vector3(1, 0, 0);
265 else if (i == 1) 265 else if (i == 1)
266 localNormalInA = new Vector3(0, 1, 0); 266 localNormalInA = new Vector3(0, 1, 0);
267 else 267 else
268 localNormalInA = new Vector3(0, 0, 1); 268 localNormalInA = new Vector3(0, 0, 1);
269 269
270 Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform); 270 Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);
271 271
272 float jacDiagABInv = 1f / _jacLinear[i].Diagonal; 272 float jacDiagABInv = 1f / _jacLinear[i].Diagonal;
273 273
274 //velocity error (first order error) 274 //velocity error (first order error)
275 float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, 275 float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
276 RigidBodyB.LinearVelocity, angvelB); 276 RigidBodyB.LinearVelocity, angvelB);
277 277
278 //positional error (zeroth order error) 278 //positional error (zeroth order error)
279 float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld); 279 float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld);
280 float lo = -1e30f; 280 float lo = -1e30f;
281 float hi = 1e30f; 281 float hi = 1e30f;
282 282
283 //handle the limits 283 //handle the limits
284 if (_lowerLimit[i] < _upperLimit[i]) 284 if (_lowerLimit[i] < _upperLimit[i])
285 { 285 {
286 if (depth > _upperLimit[i]) 286 if (depth > _upperLimit[i])
287 { 287 {
288 depth -= _upperLimit[i]; 288 depth -= _upperLimit[i];
289 lo = 0f; 289 lo = 0f;
290 } 290 }
291 else 291 else
292 { 292 {
293 if (depth < _lowerLimit[i]) 293 if (depth < _lowerLimit[i])
294 { 294 {
295 depth -= _lowerLimit[i]; 295 depth -= _lowerLimit[i];
296 hi = 0f; 296 hi = 0f;
297 } 297 }
298 else 298 else
299 { 299 {
300 continue; 300 continue;
301 } 301 }
302 } 302 }
303 } 303 }
304 304
305 float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv; 305 float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv;
306 float oldNormalImpulse = _accumulatedImpulse[i]; 306 float oldNormalImpulse = _accumulatedImpulse[i];
307 float sum = oldNormalImpulse + normalImpulse; 307 float sum = oldNormalImpulse + normalImpulse;
308 _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum; 308 _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum;
309 normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse; 309 normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse;
310 310
311 Vector3 impulse_vector = normalWorld * normalImpulse; 311 Vector3 impulse_vector = normalWorld * normalImpulse;
312 RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1); 312 RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
313 RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2); 313 RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
314 } 314 }
315 } 315 }
316 316
317 Vector3 axis; 317 Vector3 axis;
318 float angle; 318 float angle;
319 Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA; 319 Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA;
320 Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB; 320 Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB;
321 321
322 TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle); 322 TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle);
323 Quaternion diff = new Quaternion(axis, angle); 323 Quaternion diff = new Quaternion(axis, angle);
324 Matrix diffMat = Matrix.CreateFromQuaternion(diff); 324 Matrix diffMat = Matrix.CreateFromQuaternion(diff);
325 Vector3 xyz; 325 Vector3 xyz;
326 // this is not perfect, we can first check which axis are limited, and choose a more appropriate order 326 // this is not perfect, we can first check which axis are limited, and choose a more appropriate order
327 MatrixToEulerXYZ(diffMat, out xyz); 327 MatrixToEulerXYZ(diffMat, out xyz);
328 328
329 // angular 329 // angular
330 for (int i = 0; i < 3; i++) 330 for (int i = 0; i < 3; i++)
331 { 331 {
332 if (IsLimited(i + 3)) 332 if (IsLimited(i + 3))
333 { 333 {
334 Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform)); 334 Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
335 Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform)); 335 Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));
336 336
337 float jacDiagABInv = 1f / _jacAng[i].Diagonal; 337 float jacDiagABInv = 1f / _jacAng[i].Diagonal;
338 338
339 //velocity error (first order error) 339 //velocity error (first order error)
340 float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA, 340 float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
341 RigidBodyB.LinearVelocity, angvelB); 341 RigidBodyB.LinearVelocity, angvelB);
342 342
343 //positional error (zeroth order error) 343 //positional error (zeroth order error)
344 Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform); 344 Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);
345 Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform); 345 Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);
346 346
347 float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB); 347 float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB);
348 348
349 float lo = -1e30f; 349 float lo = -1e30f;
350 float hi = 1e30f; 350 float hi = 1e30f;
351 351
352 //handle the twist limit 352 //handle the twist limit
353 if (_lowerLimit[i + 3] < _upperLimit[i + 3]) 353 if (_lowerLimit[i + 3] < _upperLimit[i + 3])
354 { 354 {
355 //clamp the values 355 //clamp the values
356 float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f; 356 float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f;
357 float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f; 357 float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f;
358 358
359 float projAngle; 359 float projAngle;
360 360
361 if (i == 0) 361 if (i == 0)
362 projAngle = -2f * xyz.Z; 362 projAngle = -2f * xyz.Z;
363 else if (i == 1) 363 else if (i == 1)
364 projAngle = -2f * xyz.Y; 364 projAngle = -2f * xyz.Y;
365 else 365 else
366 projAngle = -2f * xyz.Z; 366 projAngle = -2f * xyz.Z;
367 367
368 if (projAngle < loLimit) 368 if (projAngle < loLimit)
369 { 369 {
370 hi = 0f; 370 hi = 0f;
371 rel_pos = loLimit - projAngle; 371 rel_pos = loLimit - projAngle;
372 } 372 }
373 else 373 else
374 { 374 {
375 if (projAngle > hiLimit) 375 if (projAngle > hiLimit)
376 { 376 {
377 lo = 0f; 377 lo = 0f;
378 rel_pos = (hiLimit - projAngle); 378 rel_pos = (hiLimit - projAngle);
379 } 379 }
380 else 380 else
381 { 381 {
382 continue; 382 continue;
383 } 383 }
384 } 384 }
385 } 385 }
386 386
387 //impulse 387 //impulse
388 388
389 float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv; 389 float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv;
390 float oldNormalImpulse = _accumulatedImpulse[i + 3]; 390 float oldNormalImpulse = _accumulatedImpulse[i + 3];
391 float sum = oldNormalImpulse + normalImpulse; 391 float sum = oldNormalImpulse + normalImpulse;
392 _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum; 392 _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum;
393 normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse; 393 normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse;
394 394
395 Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB); 395 Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB);
396 Vector3 impulse_vector = axis2 * normalImpulse; 396 Vector3 impulse_vector = axis2 * normalImpulse;
397 397
398 RigidBodyA.ApplyTorqueImpulse(impulse_vector); 398 RigidBodyA.ApplyTorqueImpulse(impulse_vector);
399 RigidBodyB.ApplyTorqueImpulse(-impulse_vector); 399 RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
400 } 400 }
401 } 401 }
402 } 402 }
403 403
404 //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html 404 //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
405 private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz) 405 private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz)
406 { 406 {
407 // rot = cy*cz -cy*sz sy 407 // rot = cy*cz -cy*sz sy
408 // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx 408 // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
409 // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy 409 // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
410 xyz = new Vector3(); 410 xyz = new Vector3();
411 411
412 if (MathHelper.GetElement(mat, 2) < 1.0f) 412 if (MathHelper.GetElement(mat, 2) < 1.0f)
413 { 413 {
414 if (MathHelper.GetElement(mat, 2) > -1.0f) 414 if (MathHelper.GetElement(mat, 2) > -1.0f)
415 { 415 {
416 xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8)); 416 xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8));
417 xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2)); 417 xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2));
418 xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0)); 418 xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0));
419 return true; 419 return true;
420 } 420 }
421 else 421 else
422 { 422 {
423 // WARNING. Not unique. XA - ZA = -atan2(r10,r11) 423 // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
424 xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); 424 xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));
425 xyz.Y = -(float)Math.PI / 2; 425 xyz.Y = -(float)Math.PI / 2;
426 xyz.Z = 0.0f; 426 xyz.Z = 0.0f;
427 return false; 427 return false;
428 } 428 }
429 } 429 }
430 else 430 else
431 { 431 {
432 // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) 432 // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
433 xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4)); 433 xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));
434 xyz.Y = (float)Math.PI / 2; 434 xyz.Y = (float)Math.PI / 2;
435 xyz.Z = 0.0f; 435 xyz.Z = 0.0f;
436 return false; 436 return false;
437 } 437 }
438 } 438 }
439 } 439 }
440} 440}
generated by cgit v1.1 at 2024-08-24 04:29:30 +0000