diff options
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs | 1830 |
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 | ||
22 | using System; | 22 | using System; |
23 | using System.Collections.Generic; | 23 | using System.Collections.Generic; |
24 | using System.Text; | 24 | using System.Text; |
25 | using MonoXnaCompactMaths; | 25 | using MonoXnaCompactMaths; |
26 | 26 | ||
27 | namespace XnaDevRu.BulletX.Dynamics | 27 | namespace 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 | } |