aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs1830
1 files changed, 915 insertions, 915 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
index 4d5e93d..f275550 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
@@ -1,915 +1,915 @@
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 [Flags] 29 [Flags]
30 public enum SolverMode 30 public enum SolverMode
31 { 31 {
32 None = 0, 32 None = 0,
33 RandomizeOrder = 1, 33 RandomizeOrder = 1,
34 FrictionSeperate = 2, 34 FrictionSeperate = 2,
35 UseWarmstarting = 4, 35 UseWarmstarting = 4,
36 CacheFriendly = 8, 36 CacheFriendly = 8,
37 } 37 }
38 38
39 public class SequentialImpulseConstraintSolver : IConstraintSolver 39 public class SequentialImpulseConstraintSolver : IConstraintSolver
40 { 40 {
41 private static int _totalContactPoints = 0; 41 private static int _totalContactPoints = 0;
42 42
43 private SolverMode _solverMode; 43 private SolverMode _solverMode;
44 private int _totalCpd = 0; 44 private int _totalCpd = 0;
45 private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; 45 private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];
46 private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType]; 46 private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];
47 47
48 private float _penetrationResolveFactor = 0.9f; 48 private float _penetrationResolveFactor = 0.9f;
49 private List<SolverBody> _tmpSolverBodyPool = new List<SolverBody>(); 49 private List<SolverBody> _tmpSolverBodyPool = new List<SolverBody>();
50 private List<SolverConstraint> _tmpSolverConstraintPool = new List<SolverConstraint>(); 50 private List<SolverConstraint> _tmpSolverConstraintPool = new List<SolverConstraint>();
51 private List<SolverConstraint> _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(); 51 private List<SolverConstraint> _tmpSolverFrictionConstraintPool = new List<SolverConstraint>();
52 52
53 private const int _sequentialImpulseMaxSolverPoints = 16384; 53 private const int _sequentialImpulseMaxSolverPoints = 16384;
54 private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints]; 54 private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints];
55 private static long _seed2 = 0; 55 private static long _seed2 = 0;
56 56
57 public SequentialImpulseConstraintSolver() 57 public SequentialImpulseConstraintSolver()
58 { 58 {
59 _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly; 59 _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly;
60 PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback; 60 PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback;
61 61
62 //initialize default friction/contact funcs 62 //initialize default friction/contact funcs
63 int i, j; 63 int i, j;
64 for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++) 64 for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++)
65 for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++) 65 for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++)
66 { 66 {
67 67
68 _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision; 68 _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision;
69 _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction; 69 _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction;
70 } 70 }
71 } 71 }
72 72
73 public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } } 73 public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } }
74 public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } } 74 public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } }
75 protected static OrderIndex[] Order { get { return _order; } set { _order = value; } } 75 protected static OrderIndex[] Order { get { return _order; } set { _order = value; } }
76 public static long RandSeed { get { return _seed2; } set { _seed2 = value; } } 76 public static long RandSeed { get { return _seed2; } set { _seed2 = value; } }
77 77
78 ///<summary> 78 ///<summary>
79 /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody 79 /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
80 /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType 80 /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
81 ///</summary> 81 ///</summary>
82 public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB) 82 public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB)
83 { 83 {
84 _contactDispatch[typeA, typeB] = func; 84 _contactDispatch[typeA, typeB] = func;
85 } 85 }
86 86
87 /// <summary> 87 /// <summary>
88 /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody 88 /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
89 /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType 89 /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
90 ///</summary> 90 ///</summary>
91 public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB) 91 public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB)
92 { 92 {
93 _frictionDispatch[typeA, typeB] = func; 93 _frictionDispatch[typeA, typeB] = func;
94 } 94 }
95 95
96 protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) 96 protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
97 { 97 {
98 float maxImpulse = 0; 98 float maxImpulse = 0;
99 99
100 Vector3 color = new Vector3(0, 1, 0); 100 Vector3 color = new Vector3(0, 1, 0);
101 if (cp.Distance <= 0) 101 if (cp.Distance <= 0)
102 { 102 {
103 if (iter == 0) 103 if (iter == 0)
104 if(debugDraw != null) 104 if(debugDraw != null)
105 debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); 105 debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
106 106
107 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; 107 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
108 float impulse = cpd.ContactSolverFunc( 108 float impulse = cpd.ContactSolverFunc(
109 bodyA, bodyB, 109 bodyA, bodyB,
110 cp, 110 cp,
111 info); 111 info);
112 112
113 if (maxImpulse < impulse) 113 if (maxImpulse < impulse)
114 maxImpulse = impulse; 114 maxImpulse = impulse;
115 } 115 }
116 return maxImpulse; 116 return maxImpulse;
117 } 117 }
118 118
119 protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter) 119 protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter)
120 { 120 {
121 return Solve(bodyA, bodyB, cp, info, iter, null); 121 return Solve(bodyA, bodyB, cp, info, iter, null);
122 } 122 }
123 123
124 protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) 124 protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
125 { 125 {
126 float maxImpulse = 0; 126 float maxImpulse = 0;
127 127
128 Vector3 color = new Vector3(0, 1, 0); 128 Vector3 color = new Vector3(0, 1, 0);
129 if (cp.Distance <= 0) 129 if (cp.Distance <= 0)
130 { 130 {
131 if (iter == 0) 131 if (iter == 0)
132 if (debugDraw != null) 132 if (debugDraw != null)
133 debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color); 133 debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
134 134
135 float impulse = ContactConstraint.ResolveSingleCollisionCombined( 135 float impulse = ContactConstraint.ResolveSingleCollisionCombined(
136 bodyA, bodyB, 136 bodyA, bodyB,
137 cp, 137 cp,
138 info); 138 info);
139 139
140 if (maxImpulse < impulse) 140 if (maxImpulse < impulse)
141 maxImpulse = impulse; 141 maxImpulse = impulse;
142 } 142 }
143 return maxImpulse; 143 return maxImpulse;
144 } 144 }
145 145
146 protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw) 146 protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
147 { 147 {
148 Vector3 color = new Vector3(0, 1, 0); 148 Vector3 color = new Vector3(0, 1, 0);
149 149
150 if (cp.Distance <= 0) 150 if (cp.Distance <= 0)
151 { 151 {
152 152
153 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; 153 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
154 cpd.FrictionSolverFunc( 154 cpd.FrictionSolverFunc(
155 bodyA, bodyB, 155 bodyA, bodyB,
156 cp, 156 cp,
157 info); 157 info);
158 } 158 }
159 return 0; 159 return 0;
160 } 160 }
161 161
162 protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info) 162 protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info)
163 { 163 {
164 RigidBody body0 = manifold.BodyA as RigidBody; 164 RigidBody body0 = manifold.BodyA as RigidBody;
165 RigidBody body1 = manifold.BodyB as RigidBody; 165 RigidBody body1 = manifold.BodyB as RigidBody;
166 166
167 167
168 //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop 168 //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
169 { 169 {
170 manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform); 170 manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform);
171 171
172 int numpoints = manifold.ContactsCount; 172 int numpoints = manifold.ContactsCount;
173 173
174 _totalContactPoints += numpoints; 174 _totalContactPoints += numpoints;
175 175
176 Vector3 color = new Vector3(0, 1, 0); 176 Vector3 color = new Vector3(0, 1, 0);
177 for (int i = 0; i < numpoints; i++) 177 for (int i = 0; i < numpoints; i++)
178 { 178 {
179 ManifoldPoint cp = manifold.GetContactPoint(i); 179 ManifoldPoint cp = manifold.GetContactPoint(i);
180 if (cp.Distance <= 0) 180 if (cp.Distance <= 0)
181 { 181 {
182 Vector3 pos1 = cp.PositionWorldOnA; 182 Vector3 pos1 = cp.PositionWorldOnA;
183 Vector3 pos2 = cp.PositionWorldOnB; 183 Vector3 pos2 = cp.PositionWorldOnB;
184 184
185 Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition; 185 Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition;
186 Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition; 186 Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition;
187 187
188 188
189 //this jacobian entry is re-used for all iterations 189 //this jacobian entry is re-used for all iterations
190 JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform), 190 JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform),
191 MatrixOperations.Transpose(body1.CenterOfMassTransform), 191 MatrixOperations.Transpose(body1.CenterOfMassTransform),
192 rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass, 192 rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass,
193 body1.InvInertiaDiagLocal, body1.InverseMass); 193 body1.InvInertiaDiagLocal, body1.InverseMass);
194 194
195 float jacDiagAB = jac.Diagonal; 195 float jacDiagAB = jac.Diagonal;
196 196
197 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData; 197 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
198 if (cpd != null) 198 if (cpd != null)
199 { 199 {
200 //might be invalid 200 //might be invalid
201 cpd.PersistentLifeTime++; 201 cpd.PersistentLifeTime++;
202 if (cpd.PersistentLifeTime != cp.LifeTime) 202 if (cpd.PersistentLifeTime != cp.LifeTime)
203 { 203 {
204 //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); 204 //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
205 cpd = new ConstraintPersistentData(); 205 cpd = new ConstraintPersistentData();
206 cpd.PersistentLifeTime = cp.LifeTime; 206 cpd.PersistentLifeTime = cp.LifeTime;
207 207
208 } 208 }
209 } 209 }
210 else 210 else
211 { 211 {
212 212
213 cpd = new ConstraintPersistentData(); 213 cpd = new ConstraintPersistentData();
214 _totalCpd++; 214 _totalCpd++;
215 //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); 215 //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
216 cp.UserPersistentData = cpd; 216 cp.UserPersistentData = cpd;
217 cpd.PersistentLifeTime = cp.LifeTime; 217 cpd.PersistentLifeTime = cp.LifeTime;
218 //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); 218 //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
219 219
220 } 220 }
221 if (cpd == null) 221 if (cpd == null)
222 throw new BulletException(); 222 throw new BulletException();
223 223
224 cpd.JacDiagABInv = 1f / jacDiagAB; 224 cpd.JacDiagABInv = 1f / jacDiagAB;
225 225
226 //Dependent on Rigidbody A and B types, fetch the contact/friction response func 226 //Dependent on Rigidbody A and B types, fetch the contact/friction response func
227 //perhaps do a similar thing for friction/restutution combiner funcs... 227 //perhaps do a similar thing for friction/restutution combiner funcs...
228 228
229 cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType]; 229 cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType];
230 cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType]; 230 cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType];
231 231
232 Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1); 232 Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
233 Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2); 233 Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
234 Vector3 vel = vel1 - vel2; 234 Vector3 vel = vel1 - vel2;
235 float rel_vel; 235 float rel_vel;
236 rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); 236 rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
237 237
238 float combinedRestitution = cp.CombinedRestitution; 238 float combinedRestitution = cp.CombinedRestitution;
239 239
240 cpd.Penetration = cp.Distance; 240 cpd.Penetration = cp.Distance;
241 cpd.Friction = cp.CombinedFriction; 241 cpd.Friction = cp.CombinedFriction;
242 cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution); 242 cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution);
243 if (cpd.Restitution < 0f) 243 if (cpd.Restitution < 0f)
244 { 244 {
245 cpd.Restitution = 0.0f; 245 cpd.Restitution = 0.0f;
246 246
247 }; 247 };
248 248
249 //restitution and penetration work in same direction so 249 //restitution and penetration work in same direction so
250 //rel_vel 250 //rel_vel
251 251
252 float penVel = -cpd.Penetration / info.TimeStep; 252 float penVel = -cpd.Penetration / info.TimeStep;
253 253
254 if (cpd.Restitution > penVel) 254 if (cpd.Restitution > penVel)
255 { 255 {
256 cpd.Penetration = 0; 256 cpd.Penetration = 0;
257 } 257 }
258 258
259 259
260 float relaxation = info.Damping; 260 float relaxation = info.Damping;
261 if ((_solverMode & SolverMode.UseWarmstarting) != 0) 261 if ((_solverMode & SolverMode.UseWarmstarting) != 0)
262 { 262 {
263 cpd.AppliedImpulse *= relaxation; 263 cpd.AppliedImpulse *= relaxation;
264 } 264 }
265 else 265 else
266 { 266 {
267 cpd.AppliedImpulse = 0f; 267 cpd.AppliedImpulse = 0f;
268 } 268 }
269 269
270 //for friction 270 //for friction
271 cpd.PreviousAppliedImpulse = cpd.AppliedImpulse; 271 cpd.PreviousAppliedImpulse = cpd.AppliedImpulse;
272 272
273 //re-calculate friction direction every frame, todo: check if this is really needed 273 //re-calculate friction direction every frame, todo: check if this is really needed
274 Vector3 fwta = cpd.FrictionWorldTangentialA; 274 Vector3 fwta = cpd.FrictionWorldTangentialA;
275 Vector3 fwtb = cpd.FrictionWorldTangentialB; 275 Vector3 fwtb = cpd.FrictionWorldTangentialB;
276 MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb); 276 MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb);
277 cpd.FrictionWorldTangentialA = fwta; 277 cpd.FrictionWorldTangentialA = fwta;
278 cpd.FrictionWorldTangentialB = fwtb; 278 cpd.FrictionWorldTangentialB = fwtb;
279 279
280 cpd.AccumulatedTangentImpulseA = 0; 280 cpd.AccumulatedTangentImpulseA = 0;
281 cpd.AccumulatedTangentImpulseB = 0; 281 cpd.AccumulatedTangentImpulseB = 0;
282 float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA); 282 float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA);
283 float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA); 283 float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA);
284 float denom = relaxation / (denom0 + denom1); 284 float denom = relaxation / (denom0 + denom1);
285 cpd.JacDiagABInvTangentA = denom; 285 cpd.JacDiagABInvTangentA = denom;
286 286
287 287
288 denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB); 288 denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB);
289 denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB); 289 denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB);
290 denom = relaxation / (denom0 + denom1); 290 denom = relaxation / (denom0 + denom1);
291 cpd.JacDiagABInvTangentB = denom; 291 cpd.JacDiagABInvTangentB = denom;
292 292
293 Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse; 293 Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse;
294 294
295 { 295 {
296 Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); 296 Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
297 cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld); 297 cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld);
298 Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); 298 Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
299 cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld); 299 cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld);
300 } 300 }
301 { 301 {
302 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA); 302 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA);
303 cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld); 303 cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld);
304 } 304 }
305 { 305 {
306 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB); 306 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB);
307 cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld); 307 cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld);
308 } 308 }
309 { 309 {
310 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA); 310 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA);
311 cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld); 311 cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld);
312 } 312 }
313 { 313 {
314 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB); 314 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB);
315 cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld); 315 cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld);
316 } 316 }
317 317
318 318
319 //apply previous frames impulse on both bodies 319 //apply previous frames impulse on both bodies
320 body0.ApplyImpulse(totalImpulse, rel_pos1); 320 body0.ApplyImpulse(totalImpulse, rel_pos1);
321 body1.ApplyImpulse(-totalImpulse, rel_pos2); 321 body1.ApplyImpulse(-totalImpulse, rel_pos2);
322 } 322 }
323 } 323 }
324 } 324 }
325 } 325 }
326 326
327 private bool MyContactDestroyedCallback(object userPersistentData) 327 private bool MyContactDestroyedCallback(object userPersistentData)
328 { 328 {
329 if (userPersistentData == null) 329 if (userPersistentData == null)
330 throw new BulletException(); 330 throw new BulletException();
331 ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData; 331 ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData;
332 _totalCpd--; 332 _totalCpd--;
333 return true; 333 return true;
334 } 334 }
335 335
336 private float RestitutionCurve(float relVel, float restitution) 336 private float RestitutionCurve(float relVel, float restitution)
337 { 337 {
338 float rest = restitution * -relVel; 338 float rest = restitution * -relVel;
339 return rest; 339 return rest;
340 } 340 }
341 341
342 //velocity + friction 342 //velocity + friction
343 //response between two dynamic objects with friction 343 //response between two dynamic objects with friction
344 public virtual float ResolveSingleCollisionCombinedCacheFriendly( 344 public virtual float ResolveSingleCollisionCombinedCacheFriendly(
345 SolverBody bodyA, 345 SolverBody bodyA,
346 SolverBody bodyB, 346 SolverBody bodyB,
347 SolverConstraint contactConstraint, 347 SolverConstraint contactConstraint,
348 ContactSolverInfo solverInfo) 348 ContactSolverInfo solverInfo)
349 { 349 {
350 float normalImpulse = 0; 350 float normalImpulse = 0;
351 351
352 if (contactConstraint.Penetration < 0) 352 if (contactConstraint.Penetration < 0)
353 return 0; 353 return 0;
354 354
355 float relVel; 355 float relVel;
356 float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity) 356 float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity)
357 + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity); 357 + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity);
358 float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity) 358 float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity)
359 + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity); 359 + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity);
360 360
361 relVel = velADotn - velBDotn; 361 relVel = velADotn - velBDotn;
362 362
363 float positionalError = contactConstraint.Penetration; 363 float positionalError = contactConstraint.Penetration;
364 float velocityError = contactConstraint.Restitution - relVel;// * damping; 364 float velocityError = contactConstraint.Restitution - relVel;// * damping;
365 365
366 float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv; 366 float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;
367 float velocityImpulse = velocityError * contactConstraint.JacDiagABInv; 367 float velocityImpulse = velocityError * contactConstraint.JacDiagABInv;
368 normalImpulse = penetrationImpulse + velocityImpulse; 368 normalImpulse = penetrationImpulse + velocityImpulse;
369 369
370 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse 370 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
371 float oldNormalImpulse = contactConstraint.AppliedImpulse; 371 float oldNormalImpulse = contactConstraint.AppliedImpulse;
372 float sum = oldNormalImpulse + normalImpulse; 372 float sum = oldNormalImpulse + normalImpulse;
373 contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum; 373 contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;
374 374
375 float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse; 375 float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;
376 float velocitySum = oldVelocityImpulse + velocityImpulse; 376 float velocitySum = oldVelocityImpulse + velocityImpulse;
377 contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum; 377 contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;
378 378
379 normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse; 379 normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;
380 380
381 if (bodyA.InvMass != 0) 381 if (bodyA.InvMass != 0)
382 { 382 {
383 bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, 383 bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,
384 contactConstraint.AngularComponentA, normalImpulse); 384 contactConstraint.AngularComponentA, normalImpulse);
385 } 385 }
386 if (bodyB.InvMass != 0) 386 if (bodyB.InvMass != 0)
387 { 387 {
388 bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, 388 bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,
389 contactConstraint.AngularComponentB, -normalImpulse); 389 contactConstraint.AngularComponentB, -normalImpulse);
390 } 390 }
391 391
392 return normalImpulse; 392 return normalImpulse;
393 } 393 }
394 394
395 public virtual float ResolveSingleFrictionCacheFriendly( 395 public virtual float ResolveSingleFrictionCacheFriendly(
396 SolverBody bodyA, 396 SolverBody bodyA,
397 SolverBody bodyB, 397 SolverBody bodyB,
398 SolverConstraint contactConstraint, 398 SolverConstraint contactConstraint,
399 ContactSolverInfo solverInfo, 399 ContactSolverInfo solverInfo,
400 float appliedNormalImpulse) 400 float appliedNormalImpulse)
401 { 401 {
402 float combinedFriction = contactConstraint.Friction; 402 float combinedFriction = contactConstraint.Friction;
403 float limit = appliedNormalImpulse * combinedFriction; 403 float limit = appliedNormalImpulse * combinedFriction;
404 404
405 if (appliedNormalImpulse > 0) 405 if (appliedNormalImpulse > 0)
406 //friction 406 //friction
407 { 407 {
408 float j1; 408 float j1;
409 { 409 {
410 float relVel; 410 float relVel;
411 float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity) 411 float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
412 + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity); 412 + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
413 float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity) 413 float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
414 + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity); 414 + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
415 relVel = velADotn - velBDotn; 415 relVel = velADotn - velBDotn;
416 416
417 // calculate j that moves us to zero relative velocity 417 // calculate j that moves us to zero relative velocity
418 j1 = -relVel * contactConstraint.JacDiagABInv; 418 j1 = -relVel * contactConstraint.JacDiagABInv;
419 float oldTangentImpulse = contactConstraint.AppliedImpulse; 419 float oldTangentImpulse = contactConstraint.AppliedImpulse;
420 contactConstraint.AppliedImpulse = oldTangentImpulse + j1; 420 contactConstraint.AppliedImpulse = oldTangentImpulse + j1;
421 421
422 float test = contactConstraint.AppliedImpulse; 422 float test = contactConstraint.AppliedImpulse;
423 MathHelper.SetMin(ref test, limit); 423 MathHelper.SetMin(ref test, limit);
424 MathHelper.SetMax(ref test, -limit); 424 MathHelper.SetMax(ref test, -limit);
425 contactConstraint.AppliedImpulse = test; 425 contactConstraint.AppliedImpulse = test;
426 426
427 j1 = contactConstraint.AppliedImpulse - oldTangentImpulse; 427 j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;
428 } 428 }
429 429
430 if (bodyA.InvMass != 0) 430 if (bodyA.InvMass != 0)
431 { 431 {
432 bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1); 432 bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);
433 } 433 }
434 if (bodyB.InvMass != 0) 434 if (bodyB.InvMass != 0)
435 { 435 {
436 bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1); 436 bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);
437 } 437 }
438 } 438 }
439 return 0; 439 return 0;
440 } 440 }
441 441
442 public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) 442 public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
443 { 443 {
444 if (constraints.Count + numManifolds == 0) 444 if (constraints.Count + numManifolds == 0)
445 { 445 {
446 return 0; 446 return 0;
447 } 447 }
448 448
449 for (int i = 0; i < numManifolds; i++) 449 for (int i = 0; i < numManifolds; i++)
450 { 450 {
451 PersistentManifold manifold = manifolds[i]; 451 PersistentManifold manifold = manifolds[i];
452 RigidBody rbA = (RigidBody)manifold.BodyA; 452 RigidBody rbA = (RigidBody)manifold.BodyA;
453 RigidBody rbB = (RigidBody)manifold.BodyB; 453 RigidBody rbB = (RigidBody)manifold.BodyB;
454 454
455 manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform); 455 manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform);
456 } 456 }
457 457
458 int minReservation = manifolds.Count * 2; 458 int minReservation = manifolds.Count * 2;
459 459
460 _tmpSolverBodyPool = new List<SolverBody>(minReservation); 460 _tmpSolverBodyPool = new List<SolverBody>(minReservation);
461 461
462 for (int i = 0; i < bodies.Count; i++) 462 for (int i = 0; i < bodies.Count; i++)
463 { 463 {
464 RigidBody rb = RigidBody.Upcast(bodies[i]); 464 RigidBody rb = RigidBody.Upcast(bodies[i]);
465 if (rb != null && rb.IslandTag >= 0) 465 if (rb != null && rb.IslandTag >= 0)
466 { 466 {
467 BulletDebug.Assert(rb.CompanionID < 0); 467 BulletDebug.Assert(rb.CompanionID < 0);
468 int solverBodyId = _tmpSolverBodyPool.Count; 468 int solverBodyId = _tmpSolverBodyPool.Count;
469 SolverBody solverBody; 469 SolverBody solverBody;
470 InitSolverBody(out solverBody, rb); 470 InitSolverBody(out solverBody, rb);
471 _tmpSolverBodyPool.Add(solverBody); 471 _tmpSolverBodyPool.Add(solverBody);
472 rb.CompanionID = solverBodyId; 472 rb.CompanionID = solverBodyId;
473 } 473 }
474 } 474 }
475 475
476 _tmpSolverConstraintPool = new List<SolverConstraint>(minReservation); 476 _tmpSolverConstraintPool = new List<SolverConstraint>(minReservation);
477 _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation); 477 _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation);
478 478
479 for (int i = 0; i < numManifolds; i++) 479 for (int i = 0; i < numManifolds; i++)
480 { 480 {
481 PersistentManifold manifold = manifolds[i]; 481 PersistentManifold manifold = manifolds[i];
482 RigidBody rb0 = (RigidBody)manifold.BodyA; 482 RigidBody rb0 = (RigidBody)manifold.BodyA;
483 RigidBody rb1 = (RigidBody)manifold.BodyB; 483 RigidBody rb1 = (RigidBody)manifold.BodyB;
484 484
485 int solverBodyIdA = -1; 485 int solverBodyIdA = -1;
486 int solverBodyIdB = -1; 486 int solverBodyIdB = -1;
487 487
488 //if (i == 89) 488 //if (i == 89)
489 // System.Diagnostics.Debugger.Break(); 489 // System.Diagnostics.Debugger.Break();
490 490
491 if (manifold.ContactsCount != 0) 491 if (manifold.ContactsCount != 0)
492 { 492 {
493 if (rb0.IslandTag >= 0) 493 if (rb0.IslandTag >= 0)
494 { 494 {
495 solverBodyIdA = rb0.CompanionID; 495 solverBodyIdA = rb0.CompanionID;
496 } 496 }
497 else 497 else
498 { 498 {
499 //create a static body 499 //create a static body
500 solverBodyIdA = _tmpSolverBodyPool.Count; 500 solverBodyIdA = _tmpSolverBodyPool.Count;
501 SolverBody solverBody; 501 SolverBody solverBody;
502 InitSolverBody(out solverBody, rb0); 502 InitSolverBody(out solverBody, rb0);
503 _tmpSolverBodyPool.Add(solverBody); 503 _tmpSolverBodyPool.Add(solverBody);
504 } 504 }
505 505
506 if (rb1.IslandTag >= 0) 506 if (rb1.IslandTag >= 0)
507 { 507 {
508 solverBodyIdB = rb1.CompanionID; 508 solverBodyIdB = rb1.CompanionID;
509 } 509 }
510 else 510 else
511 { 511 {
512 //create a static body 512 //create a static body
513 solverBodyIdB = _tmpSolverBodyPool.Count; 513 solverBodyIdB = _tmpSolverBodyPool.Count;
514 SolverBody solverBody; 514 SolverBody solverBody;
515 InitSolverBody(out solverBody, rb1); 515 InitSolverBody(out solverBody, rb1);
516 _tmpSolverBodyPool.Add(solverBody); 516 _tmpSolverBodyPool.Add(solverBody);
517 } 517 }
518 } 518 }
519 519
520 if (solverBodyIdB == -1 || solverBodyIdA == -1) 520 if (solverBodyIdB == -1 || solverBodyIdA == -1)
521 System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i)); 521 System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i));
522 522
523 for (int j = 0; j < manifold.ContactsCount; j++) 523 for (int j = 0; j < manifold.ContactsCount; j++)
524 { 524 {
525 ManifoldPoint cp = manifold.GetContactPoint(j); 525 ManifoldPoint cp = manifold.GetContactPoint(j);
526 526
527 int frictionIndex = _tmpSolverConstraintPool.Count; 527 int frictionIndex = _tmpSolverConstraintPool.Count;
528 528
529 if (cp.Distance <= 0) 529 if (cp.Distance <= 0)
530 { 530 {
531 531
532 Vector3 pos1 = cp.PositionWorldOnA; 532 Vector3 pos1 = cp.PositionWorldOnA;
533 Vector3 pos2 = cp.PositionWorldOnB; 533 Vector3 pos2 = cp.PositionWorldOnB;
534 534
535 Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition; 535 Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition;
536 Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition; 536 Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition;
537 537
538 float relaxation = 1; 538 float relaxation = 1;
539 { 539 {
540 SolverConstraint solverConstraint = new SolverConstraint(); 540 SolverConstraint solverConstraint = new SolverConstraint();
541 _tmpSolverConstraintPool.Add(solverConstraint); 541 _tmpSolverConstraintPool.Add(solverConstraint);
542 542
543 solverConstraint.SolverBodyIdA = solverBodyIdA; 543 solverConstraint.SolverBodyIdA = solverBodyIdA;
544 solverConstraint.SolverBodyIdB = solverBodyIdB; 544 solverConstraint.SolverBodyIdB = solverBodyIdB;
545 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact; 545 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact;
546 546
547 //can be optimized, the cross products are already calculated 547 //can be optimized, the cross products are already calculated
548 float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB); 548 float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB);
549 float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB); 549 float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB);
550 float denom = relaxation / (denom0 + denom1); 550 float denom = relaxation / (denom0 + denom1);
551 solverConstraint.JacDiagABInv = denom; 551 solverConstraint.JacDiagABInv = denom;
552 552
553 solverConstraint.ContactNormal = cp.NormalWorldOnB; 553 solverConstraint.ContactNormal = cp.NormalWorldOnB;
554 solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); 554 solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
555 solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); 555 solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
556 556
557 Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1); 557 Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1);
558 Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2); 558 Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2);
559 559
560 Vector3 vel = vel1 - vel2; 560 Vector3 vel = vel1 - vel2;
561 float rel_vel; 561 float rel_vel;
562 rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel); 562 rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
563 563
564 564
565 solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations); 565 solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations);
566 solverConstraint.Friction = cp.CombinedFriction; 566 solverConstraint.Friction = cp.CombinedFriction;
567 float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution); 567 float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution);
568 if (rest <= 0) 568 if (rest <= 0)
569 { 569 {
570 rest = 0; 570 rest = 0;
571 } 571 }
572 572
573 float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep; 573 float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep;
574 if (rest > penVel) 574 if (rest > penVel)
575 { 575 {
576 rest = 0; 576 rest = 0;
577 } 577 }
578 solverConstraint.Restitution = rest; 578 solverConstraint.Restitution = rest;
579 579
580 solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep); 580 solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep);
581 581
582 solverConstraint.AppliedImpulse = 0f; 582 solverConstraint.AppliedImpulse = 0f;
583 solverConstraint.AppliedVelocityImpulse = 0f; 583 solverConstraint.AppliedVelocityImpulse = 0f;
584 584
585#warning Check to see if we need Vector3.Transform 585#warning Check to see if we need Vector3.Transform
586 Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB); 586 Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
587 solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld); 587 solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld);
588 Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB); 588 Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
589 solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld); 589 solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld);
590 } 590 }
591 //create 2 '1d axis' constraints for 2 tangential friction directions 591 //create 2 '1d axis' constraints for 2 tangential friction directions
592 592
593 //re-calculate friction direction every frame, todo: check if this is really needed 593 //re-calculate friction direction every frame, todo: check if this is really needed
594 Vector3 frictionTangential0a = new Vector3(), 594 Vector3 frictionTangential0a = new Vector3(),
595 frictionTangential1b = new Vector3(); 595 frictionTangential1b = new Vector3();
596 596
597 MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b); 597 MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b);
598 { 598 {
599 SolverConstraint solverConstraint = new SolverConstraint(); 599 SolverConstraint solverConstraint = new SolverConstraint();
600 _tmpSolverFrictionConstraintPool.Add(solverConstraint); 600 _tmpSolverFrictionConstraintPool.Add(solverConstraint);
601 solverConstraint.ContactNormal = frictionTangential0a; 601 solverConstraint.ContactNormal = frictionTangential0a;
602 602
603 solverConstraint.SolverBodyIdA = solverBodyIdA; 603 solverConstraint.SolverBodyIdA = solverBodyIdA;
604 solverConstraint.SolverBodyIdB = solverBodyIdB; 604 solverConstraint.SolverBodyIdB = solverBodyIdB;
605 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; 605 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
606 solverConstraint.FrictionIndex = frictionIndex; 606 solverConstraint.FrictionIndex = frictionIndex;
607 607
608 solverConstraint.Friction = cp.CombinedFriction; 608 solverConstraint.Friction = cp.CombinedFriction;
609 609
610 solverConstraint.AppliedImpulse = 0; 610 solverConstraint.AppliedImpulse = 0;
611 solverConstraint.AppliedVelocityImpulse = 0; 611 solverConstraint.AppliedVelocityImpulse = 0;
612 612
613 float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); 613 float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
614 float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); 614 float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
615 float denom = relaxation / (denom0 + denom1); 615 float denom = relaxation / (denom0 + denom1);
616 solverConstraint.JacDiagABInv = denom; 616 solverConstraint.JacDiagABInv = denom;
617 617
618 { 618 {
619 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); 619 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
620 solverConstraint.RelPosACrossNormal = ftorqueAxis0; 620 solverConstraint.RelPosACrossNormal = ftorqueAxis0;
621 solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld); 621 solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld);
622 } 622 }
623 { 623 {
624 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); 624 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
625 solverConstraint.RelPosBCrossNormal = ftorqueAxis0; 625 solverConstraint.RelPosBCrossNormal = ftorqueAxis0;
626 solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld); 626 solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld);
627 } 627 }
628 } 628 }
629 629
630 630
631 { 631 {
632 632
633 SolverConstraint solverConstraint = new SolverConstraint(); 633 SolverConstraint solverConstraint = new SolverConstraint();
634 _tmpSolverFrictionConstraintPool.Add(solverConstraint); 634 _tmpSolverFrictionConstraintPool.Add(solverConstraint);
635 solverConstraint.ContactNormal = frictionTangential1b; 635 solverConstraint.ContactNormal = frictionTangential1b;
636 636
637 solverConstraint.SolverBodyIdA = solverBodyIdA; 637 solverConstraint.SolverBodyIdA = solverBodyIdA;
638 solverConstraint.SolverBodyIdB = solverBodyIdB; 638 solverConstraint.SolverBodyIdB = solverBodyIdB;
639 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction; 639 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
640 solverConstraint.FrictionIndex = frictionIndex; 640 solverConstraint.FrictionIndex = frictionIndex;
641 641
642 solverConstraint.Friction = cp.CombinedFriction; 642 solverConstraint.Friction = cp.CombinedFriction;
643 643
644 solverConstraint.AppliedImpulse = 0; 644 solverConstraint.AppliedImpulse = 0;
645 solverConstraint.AppliedVelocityImpulse = 0; 645 solverConstraint.AppliedVelocityImpulse = 0;
646 646
647 float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal); 647 float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
648 float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal); 648 float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
649 float denom = relaxation / (denom0 + denom1); 649 float denom = relaxation / (denom0 + denom1);
650 solverConstraint.JacDiagABInv = denom; 650 solverConstraint.JacDiagABInv = denom;
651 { 651 {
652 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal); 652 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
653 solverConstraint.RelPosACrossNormal = ftorqueAxis1; 653 solverConstraint.RelPosACrossNormal = ftorqueAxis1;
654 solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld); 654 solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld);
655 } 655 }
656 { 656 {
657 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal); 657 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
658 solverConstraint.RelPosBCrossNormal = ftorqueAxis1; 658 solverConstraint.RelPosBCrossNormal = ftorqueAxis1;
659 solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld); 659 solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld);
660 } 660 }
661 } 661 }
662 } 662 }
663 } 663 }
664 } 664 }
665 665
666 ContactSolverInfo info = infoGlobal; 666 ContactSolverInfo info = infoGlobal;
667 { 667 {
668 for (int j = 0; j < constraints.Count; j++) 668 for (int j = 0; j < constraints.Count; j++)
669 { 669 {
670 TypedConstraint constraint = constraints[j]; 670 TypedConstraint constraint = constraints[j];
671 constraint.BuildJacobian(); 671 constraint.BuildJacobian();
672 } 672 }
673 } 673 }
674 674
675 int numConstraintPool = _tmpSolverConstraintPool.Count; 675 int numConstraintPool = _tmpSolverConstraintPool.Count;
676 int numFrictionPool = _tmpSolverFrictionConstraintPool.Count; 676 int numFrictionPool = _tmpSolverFrictionConstraintPool.Count;
677 677
678 //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints 678 //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
679 List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool); 679 List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool);
680 List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool); 680 List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool);
681 { 681 {
682 for (int i = 0; i < numConstraintPool; i++) 682 for (int i = 0; i < numConstraintPool; i++)
683 { 683 {
684 gOrderTmpConstraintPool.Add(i); 684 gOrderTmpConstraintPool.Add(i);
685 } 685 }
686 for (int i = 0; i < numFrictionPool; i++) 686 for (int i = 0; i < numFrictionPool; i++)
687 { 687 {
688 gOrderFrictionConstraintPool.Add(i); 688 gOrderFrictionConstraintPool.Add(i);
689 } 689 }
690 } 690 }
691 691
692 //should traverse the contacts random order... 692 //should traverse the contacts random order...
693 int iteration; 693 int iteration;
694 { 694 {
695 for (iteration = 0; iteration < info.IterationsCount; iteration++) 695 for (iteration = 0; iteration < info.IterationsCount; iteration++)
696 { 696 {
697 697
698 int j; 698 int j;
699 if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) 699 if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
700 { 700 {
701 if ((iteration & 7) == 0) 701 if ((iteration & 7) == 0)
702 { 702 {
703 for (j = 0; j < numConstraintPool; ++j) 703 for (j = 0; j < numConstraintPool; ++j)
704 { 704 {
705 int tmp = gOrderTmpConstraintPool[j]; 705 int tmp = gOrderTmpConstraintPool[j];
706 int swapi = RandInt2(j + 1); 706 int swapi = RandInt2(j + 1);
707 gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi]; 707 gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi];
708 gOrderTmpConstraintPool[swapi] = tmp; 708 gOrderTmpConstraintPool[swapi] = tmp;
709 } 709 }
710 710
711 for (j = 0; j < numFrictionPool; ++j) 711 for (j = 0; j < numFrictionPool; ++j)
712 { 712 {
713 int tmp = gOrderFrictionConstraintPool[j]; 713 int tmp = gOrderFrictionConstraintPool[j];
714 int swapi = RandInt2(j + 1); 714 int swapi = RandInt2(j + 1);
715 gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi]; 715 gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi];
716 gOrderFrictionConstraintPool[swapi] = tmp; 716 gOrderFrictionConstraintPool[swapi] = tmp;
717 } 717 }
718 } 718 }
719 } 719 }
720 720
721 for (j = 0; j < constraints.Count; j++) 721 for (j = 0; j < constraints.Count; j++)
722 { 722 {
723 TypedConstraint constraint = constraints[j]; 723 TypedConstraint constraint = constraints[j];
724 //todo: use solver bodies, so we don't need to copy from/to btRigidBody 724 //todo: use solver bodies, so we don't need to copy from/to btRigidBody
725 725
726 if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) 726 if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
727 { 727 {
728 _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity(); 728 _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity();
729 } 729 }
730 if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) 730 if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
731 { 731 {
732 _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity(); 732 _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity();
733 } 733 }
734 734
735 constraint.SolveConstraint(info.TimeStep); 735 constraint.SolveConstraint(info.TimeStep);
736 736
737 if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0)) 737 if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
738 { 738 {
739 _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity(); 739 _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity();
740 } 740 }
741 if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0)) 741 if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
742 { 742 {
743 _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity(); 743 _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity();
744 } 744 }
745 745
746 } 746 }
747 747
748 { 748 {
749 int numPoolConstraints = _tmpSolverConstraintPool.Count; 749 int numPoolConstraints = _tmpSolverConstraintPool.Count;
750 for (j = 0; j < numPoolConstraints; j++) 750 for (j = 0; j < numPoolConstraints; j++)
751 { 751 {
752 SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]]; 752 SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
753 ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], 753 ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
754 _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info); 754 _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info);
755 } 755 }
756 } 756 }
757 757
758 { 758 {
759 int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count; 759 int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count;
760 for (j = 0; j < numFrictionPoolConstraints; j++) 760 for (j = 0; j < numFrictionPoolConstraints; j++)
761 { 761 {
762 SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]]; 762 SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
763 float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse; 763 float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse;
764 764
765 ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA], 765 ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
766 _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse); 766 _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse);
767 } 767 }
768 } 768 }
769 } 769 }
770 } 770 }
771 771
772 for (int i = 0; i < _tmpSolverBodyPool.Count; i++) 772 for (int i = 0; i < _tmpSolverBodyPool.Count; i++)
773 { 773 {
774 _tmpSolverBodyPool[i].WriteBackVelocity(); 774 _tmpSolverBodyPool[i].WriteBackVelocity();
775 } 775 }
776 776
777 _tmpSolverBodyPool.Clear(); 777 _tmpSolverBodyPool.Clear();
778 _tmpSolverConstraintPool.Clear(); 778 _tmpSolverConstraintPool.Clear();
779 _tmpSolverFrictionConstraintPool.Clear(); 779 _tmpSolverFrictionConstraintPool.Clear();
780 780
781 return 0; 781 return 0;
782 } 782 }
783 783
784 public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) 784 public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
785 { 785 {
786 if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None) 786 if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None)
787 { 787 {
788 return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer); 788 return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer);
789 } 789 }
790 790
791 ContactSolverInfo info = infoGlobal; 791 ContactSolverInfo info = infoGlobal;
792 int totalPoints = 0; 792 int totalPoints = 0;
793 793
794 int numiter = infoGlobal.IterationsCount; 794 int numiter = infoGlobal.IterationsCount;
795 795
796 for (int j = 0; j < manifolds.Count; j++) 796 for (int j = 0; j < manifolds.Count; j++)
797 { 797 {
798 PersistentManifold manifold = manifolds[j]; 798 PersistentManifold manifold = manifolds[j];
799 PrepareConstraints(manifold, info); 799 PrepareConstraints(manifold, info);
800 800
801 for (int p = 0; p < manifolds[j].ContactsCount; p++) 801 for (int p = 0; p < manifolds[j].ContactsCount; p++)
802 { 802 {
803 _order[totalPoints].ManifoldIndex = j; 803 _order[totalPoints].ManifoldIndex = j;
804 _order[totalPoints].PointIndex = p; 804 _order[totalPoints].PointIndex = p;
805 totalPoints++; 805 totalPoints++;
806 } 806 }
807 } 807 }
808 808
809 for (int j = 0; j < constraints.Count; j++) 809 for (int j = 0; j < constraints.Count; j++)
810 { 810 {
811 constraints[j].BuildJacobian(); 811 constraints[j].BuildJacobian();
812 } 812 }
813 813
814 //should traverse the contacts random order... 814 //should traverse the contacts random order...
815 int iteration; 815 int iteration;
816 816
817 for (iteration = 0; iteration < numiter; iteration++) 817 for (iteration = 0; iteration < numiter; iteration++)
818 { 818 {
819 int j; 819 int j;
820 if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None) 820 if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
821 { 821 {
822 if ((iteration & 7) == 0) 822 if ((iteration & 7) == 0)
823 { 823 {
824 for (j = 0; j < totalPoints; ++j) 824 for (j = 0; j < totalPoints; ++j)
825 { 825 {
826 OrderIndex tmp = _order[j]; 826 OrderIndex tmp = _order[j];
827 int swapi = RandInt2(j + 1); 827 int swapi = RandInt2(j + 1);
828 _order[j] = _order[swapi]; 828 _order[j] = _order[swapi];
829 _order[swapi] = tmp; 829 _order[swapi] = tmp;
830 } 830 }
831 } 831 }
832 } 832 }
833 833
834 for (j = 0; j < constraints.Count; j++) 834 for (j = 0; j < constraints.Count; j++)
835 { 835 {
836 constraints[j].SolveConstraint(info.TimeStep); 836 constraints[j].SolveConstraint(info.TimeStep);
837 } 837 }
838 838
839 for (j = 0; j < totalPoints; j++) 839 for (j = 0; j < totalPoints; j++)
840 { 840 {
841 PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; 841 PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
842 Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB, 842 Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB,
843 manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); 843 manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
844 } 844 }
845 845
846 for (j = 0; j < totalPoints; j++) 846 for (j = 0; j < totalPoints; j++)
847 { 847 {
848 PersistentManifold manifold = manifolds[_order[j].ManifoldIndex]; 848 PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
849 SolveFriction((RigidBody)manifold.BodyA, 849 SolveFriction((RigidBody)manifold.BodyA,
850 (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer); 850 (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
851 } 851 }
852 } 852 }
853 853
854 return 0; 854 return 0;
855 } 855 }
856 856
857 private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody) 857 private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody)
858 { 858 {
859 solverBody = new SolverBody(); 859 solverBody = new SolverBody();
860 solverBody.AngularVelocity = rigidBody.AngularVelocity; 860 solverBody.AngularVelocity = rigidBody.AngularVelocity;
861 solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition; 861 solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition;
862 solverBody.Friction = rigidBody.Friction; 862 solverBody.Friction = rigidBody.Friction;
863 solverBody.InvMass = rigidBody.InverseMass; 863 solverBody.InvMass = rigidBody.InverseMass;
864 solverBody.LinearVelocity = rigidBody.LinearVelocity; 864 solverBody.LinearVelocity = rigidBody.LinearVelocity;
865 solverBody.OriginalBody = rigidBody; 865 solverBody.OriginalBody = rigidBody;
866 solverBody.AngularFactor = rigidBody.AngularFactor; 866 solverBody.AngularFactor = rigidBody.AngularFactor;
867 } 867 }
868 868
869 private long Rand2() 869 private long Rand2()
870 { 870 {
871 _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff; 871 _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff;
872 return _seed2; 872 return _seed2;
873 } 873 }
874 874
875 private int RandInt2(int n) 875 private int RandInt2(int n)
876 { 876 {
877 // seems good; xor-fold and modulus 877 // seems good; xor-fold and modulus
878 long un = n; 878 long un = n;
879 long r = Rand2(); 879 long r = Rand2();
880 880
881 // note: probably more aggressive than it needs to be -- might be 881 // note: probably more aggressive than it needs to be -- might be
882 // able to get away without one or two of the innermost branches. 882 // able to get away without one or two of the innermost branches.
883 if (un <= 0x00010000L) 883 if (un <= 0x00010000L)
884 { 884 {
885 r ^= (r >> 16); 885 r ^= (r >> 16);
886 if (un <= 0x00000100L) 886 if (un <= 0x00000100L)
887 { 887 {
888 r ^= (r >> 8); 888 r ^= (r >> 8);
889 if (un <= 0x00000010L) 889 if (un <= 0x00000010L)
890 { 890 {
891 r ^= (r >> 4); 891 r ^= (r >> 4);
892 if (un <= 0x00000004L) 892 if (un <= 0x00000004L)
893 { 893 {
894 r ^= (r >> 2); 894 r ^= (r >> 2);
895 if (un <= 0x00000002L) 895 if (un <= 0x00000002L)
896 { 896 {
897 r ^= (r >> 1); 897 r ^= (r >> 1);
898 } 898 }
899 } 899 }
900 } 900 }
901 } 901 }
902 } 902 }
903 return (int)(r % un); 903 return (int)(r % un);
904 } 904 }
905 905
906 protected struct OrderIndex 906 protected struct OrderIndex
907 { 907 {
908 private int _manifoldIndex; 908 private int _manifoldIndex;
909 private int _pointIndex; 909 private int _pointIndex;
910 910
911 public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } } 911 public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } }
912 public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } } 912 public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } }
913 } 913 }
914 } 914 }
915} 915}
generated by cgit v1.1 at 2024-08-24 04:13:26 +0000