aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs376
1 files changed, 188 insertions, 188 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
index 6ced783..9d4060d 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
@@ -1,188 +1,188 @@
1/* 1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru 2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com 3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4 4
5 This software is provided 'as-is', without any express or implied 5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages 6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software. 7 arising from the use of this software.
8 8
9 Permission is granted to anyone to use this software for any purpose, 9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it 10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions: 11 freely, subject to the following restrictions:
12 12
13 1. The origin of this software must not be misrepresented; you must not 13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software 14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be 15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required. 16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be 17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software. 18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution. 19 3. This notice may not be removed or altered from any source distribution.
20*/ 20*/
21 21
22using System; 22using System;
23using System.Collections.Generic; 23using System.Collections.Generic;
24using System.Text; 24using System.Text;
25using MonoXnaCompactMaths; 25using MonoXnaCompactMaths;
26 26
27namespace XnaDevRu.BulletX.Dynamics 27namespace XnaDevRu.BulletX.Dynamics
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// constraint class used for lateral tyre friction 30 /// constraint class used for lateral tyre friction
31 /// </summary> 31 /// </summary>
32 public class Solve2LinearConstraint 32 public class Solve2LinearConstraint
33 { 33 {
34 private float _tau; 34 private float _tau;
35 private float _damping; 35 private float _damping;
36 36
37 public Solve2LinearConstraint(float tau, float damping) 37 public Solve2LinearConstraint(float tau, float damping)
38 { 38 {
39 _tau = tau; 39 _tau = tau;
40 _damping = damping; 40 _damping = damping;
41 } 41 }
42 42
43 // solve unilateral constraint (equality, direct method) 43 // solve unilateral constraint (equality, direct method)
44 public void ResolveUnilateralPairConstraint( 44 public void ResolveUnilateralPairConstraint(
45 RigidBody body1, RigidBody body2, 45 RigidBody body1, RigidBody body2,
46 Matrix world2A, 46 Matrix world2A,
47 Matrix world2B, 47 Matrix world2B,
48 Vector3 invInertiaADiag, 48 Vector3 invInertiaADiag,
49 float invMassA, 49 float invMassA,
50 Vector3 linvelA, Vector3 angvelA, 50 Vector3 linvelA, Vector3 angvelA,
51 Vector3 rel_posA1, 51 Vector3 rel_posA1,
52 Vector3 invInertiaBDiag, 52 Vector3 invInertiaBDiag,
53 float invMassB, 53 float invMassB,
54 Vector3 linvelB, Vector3 angvelB, 54 Vector3 linvelB, Vector3 angvelB,
55 Vector3 rel_posA2, 55 Vector3 rel_posA2,
56 float depthA, Vector3 normalA, 56 float depthA, Vector3 normalA,
57 Vector3 rel_posB1, Vector3 rel_posB2, 57 Vector3 rel_posB1, Vector3 rel_posB2,
58 float depthB, Vector3 normalB, 58 float depthB, Vector3 normalB,
59 out float imp0, out float imp1) 59 out float imp0, out float imp1)
60 { 60 {
61 imp0 = 0; 61 imp0 = 0;
62 imp1 = 0; 62 imp1 = 0;
63 63
64 float len = Math.Abs(normalA.Length()) - 1f; 64 float len = Math.Abs(normalA.Length()) - 1f;
65 if (Math.Abs(len) >= float.Epsilon) 65 if (Math.Abs(len) >= float.Epsilon)
66 return; 66 return;
67 67
68 BulletDebug.Assert(len < float.Epsilon); 68 BulletDebug.Assert(len < float.Epsilon);
69 69
70 //this jacobian entry could be re-used for all iterations 70 //this jacobian entry could be re-used for all iterations
71 JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, 71 JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
72 invInertiaBDiag, invMassB); 72 invInertiaBDiag, invMassB);
73 JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, 73 JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
74 invInertiaBDiag, invMassB); 74 invInertiaBDiag, invMassB);
75 75
76 float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); 76 float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
77 float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); 77 float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
78 78
79 // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv 79 // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
80 float massTerm = 1f / (invMassA + invMassB); 80 float massTerm = 1f / (invMassA + invMassB);
81 81
82 // calculate rhs (or error) terms 82 // calculate rhs (or error) terms
83 float dv0 = depthA * _tau * massTerm - vel0 * _damping; 83 float dv0 = depthA * _tau * massTerm - vel0 * _damping;
84 float dv1 = depthB * _tau * massTerm - vel1 * _damping; 84 float dv1 = depthB * _tau * massTerm - vel1 * _damping;
85 85
86 float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); 86 float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
87 float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); 87 float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
88 88
89 imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; 89 imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
90 imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; 90 imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
91 } 91 }
92 92
93 // solving 2x2 lcp problem (inequality, direct solution ) 93 // solving 2x2 lcp problem (inequality, direct solution )
94 public void ResolveBilateralPairConstraint( 94 public void ResolveBilateralPairConstraint(
95 RigidBody body1, RigidBody body2, 95 RigidBody body1, RigidBody body2,
96 Matrix world2A, Matrix world2B, 96 Matrix world2A, Matrix world2B,
97 Vector3 invInertiaADiag, 97 Vector3 invInertiaADiag,
98 float invMassA, 98 float invMassA,
99 Vector3 linvelA, Vector3 angvelA, 99 Vector3 linvelA, Vector3 angvelA,
100 Vector3 rel_posA1, 100 Vector3 rel_posA1,
101 Vector3 invInertiaBDiag, 101 Vector3 invInertiaBDiag,
102 float invMassB, 102 float invMassB,
103 Vector3 linvelB, Vector3 angvelB, 103 Vector3 linvelB, Vector3 angvelB,
104 Vector3 rel_posA2, 104 Vector3 rel_posA2,
105 float depthA, Vector3 normalA, 105 float depthA, Vector3 normalA,
106 Vector3 rel_posB1, Vector3 rel_posB2, 106 Vector3 rel_posB1, Vector3 rel_posB2,
107 float depthB, Vector3 normalB, 107 float depthB, Vector3 normalB,
108 out float imp0, out float imp1) 108 out float imp0, out float imp1)
109 { 109 {
110 imp0 = 0f; 110 imp0 = 0f;
111 imp1 = 0f; 111 imp1 = 0f;
112 112
113 float len = Math.Abs(normalA.Length()) - 1f; 113 float len = Math.Abs(normalA.Length()) - 1f;
114 if (Math.Abs(len) >= float.Epsilon) 114 if (Math.Abs(len) >= float.Epsilon)
115 return; 115 return;
116 116
117 BulletDebug.Assert(len < float.Epsilon); 117 BulletDebug.Assert(len < float.Epsilon);
118 118
119 JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA, 119 JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
120 invInertiaBDiag, invMassB); 120 invInertiaBDiag, invMassB);
121 JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA, 121 JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
122 invInertiaBDiag, invMassB); 122 invInertiaBDiag, invMassB);
123 123
124 float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1)); 124 float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
125 float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1)); 125 float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
126 126
127 // calculate rhs (or error) terms 127 // calculate rhs (or error) terms
128 float dv0 = depthA * _tau - vel0 * _damping; 128 float dv0 = depthA * _tau - vel0 * _damping;
129 float dv1 = depthB * _tau - vel1 * _damping; 129 float dv1 = depthB * _tau - vel1 * _damping;
130 130
131 float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB); 131 float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
132 float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag); 132 float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
133 133
134 imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet; 134 imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
135 imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet; 135 imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
136 136
137 if (imp0 > 0.0f) 137 if (imp0 > 0.0f)
138 { 138 {
139 if (imp1 <= 0.0f) 139 if (imp1 <= 0.0f)
140 { 140 {
141 imp1 = 0f; 141 imp1 = 0f;
142 142
143 // now imp0>0 imp1<0 143 // now imp0>0 imp1<0
144 imp0 = dv0 / jacA.Diagonal; 144 imp0 = dv0 / jacA.Diagonal;
145 if (imp0 < 0.0f) 145 if (imp0 < 0.0f)
146 imp0 = 0f; 146 imp0 = 0f;
147 } 147 }
148 } 148 }
149 else 149 else
150 { 150 {
151 imp0 = 0f; 151 imp0 = 0f;
152 152
153 imp1 = dv1 / jacB.Diagonal; 153 imp1 = dv1 / jacB.Diagonal;
154 if (imp1 <= 0.0f) 154 if (imp1 <= 0.0f)
155 { 155 {
156 imp1 = 0f; 156 imp1 = 0f;
157 // now imp0>0 imp1<0 157 // now imp0>0 imp1<0
158 imp0 = dv0 / jacA.Diagonal; 158 imp0 = dv0 / jacA.Diagonal;
159 if (imp0 > 0.0f) 159 if (imp0 > 0.0f)
160 { 160 {
161 } 161 }
162 else 162 else
163 { 163 {
164 imp0 = 0f; 164 imp0 = 0f;
165 } 165 }
166 } 166 }
167 } 167 }
168 } 168 }
169 169
170 //public void ResolveAngularConstraint( 170 //public void ResolveAngularConstraint(
171 // Matrix invInertiaAWS, 171 // Matrix invInertiaAWS,
172 // float invMassA, 172 // float invMassA,
173 // Vector3 linvelA, Vector3 angvelA, 173 // Vector3 linvelA, Vector3 angvelA,
174 // Vector3 rel_posA1, 174 // Vector3 rel_posA1,
175 // Matrix invInertiaBWS, 175 // Matrix invInertiaBWS,
176 // float invMassB, 176 // float invMassB,
177 // Vector3 linvelB, Vector3 angvelB, 177 // Vector3 linvelB, Vector3 angvelB,
178 // Vector3 rel_posA2, 178 // Vector3 rel_posA2,
179 // float depthA, Vector3 normalA, 179 // float depthA, Vector3 normalA,
180 // Vector3 rel_posB1, Vector3 rel_posB2, 180 // Vector3 rel_posB1, Vector3 rel_posB2,
181 // float depthB, Vector3 normalB, 181 // float depthB, Vector3 normalB,
182 // out float imp0, out float imp1) 182 // out float imp0, out float imp1)
183 //{ 183 //{
184 // imp0 = 0; 184 // imp0 = 0;
185 // imp1 = 0; 185 // imp1 = 0;
186 //} 186 //}
187 } 187 }
188} 188}
generated by cgit v1.1 at 2024-09-16 18:13:01 +0000