diff options
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs | 246 |
1 files changed, 246 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs new file mode 100644 index 0000000..3e35550 --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs | |||
@@ -0,0 +1,246 @@ | |||
1 | /* | ||
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 | ||
4 | |||
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 | ||
7 | arising from the use of this software. | ||
8 | |||
9 | Permission is granted to anyone to use this software for any purpose, | ||
10 | including commercial applications, and to alter it and redistribute it | ||
11 | freely, subject to the following restrictions: | ||
12 | |||
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 | ||
15 | in a product, an acknowledgment in the product documentation would be | ||
16 | appreciated but is not required. | ||
17 | 2. Altered source versions must be plainly marked as such, and must not be | ||
18 | misrepresented as being the original software. | ||
19 | 3. This notice may not be removed or altered from any source distribution. | ||
20 | */ | ||
21 | |||
22 | using System; | ||
23 | using System.Collections.Generic; | ||
24 | using System.Text; | ||
25 | using MonoXnaCompactMaths; | ||
26 | |||
27 | namespace XnaDevRu.BulletX.Dynamics | ||
28 | { | ||
29 | /// <summary> | ||
30 | /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space | ||
31 | /// axis defines the orientation of the hinge axis | ||
32 | /// </summary> | ||
33 | public class HingeConstraint : TypedConstraint | ||
34 | { | ||
35 | private JacobianEntry[] _jac = new JacobianEntry[3]; //3 orthogonal linear constraints | ||
36 | private JacobianEntry[] _jacAng = new JacobianEntry[3]; //2 orthogonal angular constraints + 1 for limit/motor | ||
37 | |||
38 | private Vector3 _pivotInA; | ||
39 | private Vector3 _pivotInB; | ||
40 | private Vector3 _axisInA; | ||
41 | private Vector3 _axisInB; | ||
42 | |||
43 | private bool _angularOnly; | ||
44 | |||
45 | private float _motorTargetVelocity; | ||
46 | private float _maxMotorImpulse; | ||
47 | private bool _enableAngularMotor; | ||
48 | |||
49 | public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB) | ||
50 | : base(rbA, rbB) | ||
51 | { | ||
52 | _pivotInA = pivotInA; | ||
53 | _pivotInB = pivotInB; | ||
54 | _axisInA = axisInA; | ||
55 | _axisInB = -axisInB; | ||
56 | _angularOnly = false; | ||
57 | } | ||
58 | |||
59 | public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA) | ||
60 | : base(rbA) | ||
61 | { | ||
62 | _pivotInA = pivotInA; | ||
63 | _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA); | ||
64 | _axisInA = axisInA; | ||
65 | //fixed axis in worldspace | ||
66 | _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform); | ||
67 | _angularOnly = false; | ||
68 | } | ||
69 | |||
70 | public HingeConstraint() { } | ||
71 | |||
72 | public bool AngularOnly { set { _angularOnly = value; } } | ||
73 | |||
74 | public override void BuildJacobian() | ||
75 | { | ||
76 | AppliedImpulse = 0f; | ||
77 | |||
78 | Vector3 normal = new Vector3(); | ||
79 | |||
80 | if (!_angularOnly) | ||
81 | { | ||
82 | for (int i = 0; i < 3; i++) | ||
83 | { | ||
84 | MathHelper.SetElement(ref normal, i, 1); | ||
85 | _jac[i] = new JacobianEntry( | ||
86 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | ||
87 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | ||
88 | MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, | ||
89 | MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, | ||
90 | normal, | ||
91 | RigidBodyA.InvInertiaDiagLocal, | ||
92 | RigidBodyA.InverseMass, | ||
93 | RigidBodyB.InvInertiaDiagLocal, | ||
94 | RigidBodyB.InverseMass); | ||
95 | MathHelper.SetElement(ref normal, i, 0); | ||
96 | } | ||
97 | } | ||
98 | |||
99 | //calculate two perpendicular jointAxis, orthogonal to hingeAxis | ||
100 | //these two jointAxis require equal angular velocities for both bodies | ||
101 | //this is unused for now, it's a todo | ||
102 | Vector3 jointAxisALocal = new Vector3(); | ||
103 | Vector3 jointAxisBLocal = new Vector3(); | ||
104 | MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal); | ||
105 | |||
106 | Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform); | ||
107 | Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform); | ||
108 | Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); | ||
109 | |||
110 | _jacAng[0] = new JacobianEntry(jointAxisA, | ||
111 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | ||
112 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | ||
113 | RigidBodyA.InvInertiaDiagLocal, | ||
114 | RigidBodyB.InvInertiaDiagLocal); | ||
115 | |||
116 | _jacAng[1] = new JacobianEntry(jointAxisB, | ||
117 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | ||
118 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | ||
119 | RigidBodyA.InvInertiaDiagLocal, | ||
120 | RigidBodyB.InvInertiaDiagLocal); | ||
121 | |||
122 | _jacAng[2] = new JacobianEntry(hingeAxisWorld, | ||
123 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | ||
124 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | ||
125 | RigidBodyA.InvInertiaDiagLocal, | ||
126 | RigidBodyB.InvInertiaDiagLocal); | ||
127 | } | ||
128 | |||
129 | public override void SolveConstraint(float timeStep) | ||
130 | { | ||
131 | Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); | ||
132 | Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); | ||
133 | |||
134 | Vector3 normal = new Vector3(0, 0, 0); | ||
135 | float tau = 0.3f; | ||
136 | float damping = 1f; | ||
137 | |||
138 | //linear part | ||
139 | if (!_angularOnly) | ||
140 | { | ||
141 | for (int i = 0; i < 3; i++) | ||
142 | { | ||
143 | if (i == 0) | ||
144 | normal = new Vector3(1, 0, 0); | ||
145 | else if (i == 1) | ||
146 | normal = new Vector3(0, 1, 0); | ||
147 | else | ||
148 | normal = new Vector3(0, 0, 1); | ||
149 | |||
150 | float jacDiagABInv = 1f / _jac[i].Diagonal; | ||
151 | |||
152 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | ||
153 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | ||
154 | |||
155 | Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); | ||
156 | Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); | ||
157 | Vector3 vel = vel1 - vel2; | ||
158 | float rel_vel; | ||
159 | rel_vel = Vector3.Dot(normal, vel); | ||
160 | //positional error (zeroth order error) | ||
161 | float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal | ||
162 | float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; | ||
163 | AppliedImpulse += impulse; | ||
164 | Vector3 impulse_vector = normal * impulse; | ||
165 | RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition); | ||
166 | RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition); | ||
167 | } | ||
168 | } | ||
169 | //solve angular part | ||
170 | // get axes in world space | ||
171 | Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform); | ||
172 | Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform); | ||
173 | |||
174 | Vector3 angVelA = RigidBodyA.AngularVelocity; | ||
175 | Vector3 angVelB = RigidBodyB.AngularVelocity; | ||
176 | Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA); | ||
177 | Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB); | ||
178 | |||
179 | Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA; | ||
180 | Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB; | ||
181 | Vector3 velrelOrthog = angAOrthog - angBOrthog; | ||
182 | |||
183 | //solve angular velocity correction | ||
184 | float relaxation = 1f; | ||
185 | float len = velrelOrthog.Length(); | ||
186 | if (len > 0.00001f) | ||
187 | { | ||
188 | Vector3 normal2 = Vector3.Normalize(velrelOrthog); | ||
189 | float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + | ||
190 | RigidBodyB.ComputeAngularImpulseDenominator(normal2); | ||
191 | // scale for mass and relaxation | ||
192 | velrelOrthog *= (1f / denom) * 0.9f; | ||
193 | } | ||
194 | |||
195 | //solve angular positional correction | ||
196 | Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep); | ||
197 | float len2 = angularError.Length(); | ||
198 | if (len2 > 0.00001f) | ||
199 | { | ||
200 | Vector3 normal2 = Vector3.Normalize(angularError); | ||
201 | float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) + | ||
202 | RigidBodyB.ComputeAngularImpulseDenominator(normal2); | ||
203 | angularError *= (1f / denom2) * relaxation; | ||
204 | } | ||
205 | |||
206 | RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError); | ||
207 | RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError); | ||
208 | |||
209 | //apply motor | ||
210 | if (_enableAngularMotor) | ||
211 | { | ||
212 | //todo: add limits too | ||
213 | Vector3 angularLimit = Vector3.Zero; | ||
214 | |||
215 | Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; | ||
216 | float projRelVel = Vector3.Dot(velrel, axisA); | ||
217 | |||
218 | float desiredMotorVel = _motorTargetVelocity; | ||
219 | float motorRelvel = desiredMotorVel - projRelVel; | ||
220 | |||
221 | float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) + | ||
222 | RigidBodyB.ComputeAngularImpulseDenominator(axisA); | ||
223 | |||
224 | float unclippedMotorImpulse = (1f / denom3) * motorRelvel; | ||
225 | //todo: should clip against accumulated impulse | ||
226 | float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse; | ||
227 | clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse; | ||
228 | Vector3 motorImp = clippedMotorImpulse * axisA; | ||
229 | |||
230 | RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit); | ||
231 | RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit); | ||
232 | } | ||
233 | } | ||
234 | |||
235 | public void EnableAngularMotor(bool enableMotor, float targetVelocity, float maxMotorImpulse) | ||
236 | { | ||
237 | _enableAngularMotor = enableMotor; | ||
238 | _motorTargetVelocity = targetVelocity; | ||
239 | _maxMotorImpulse = maxMotorImpulse; | ||
240 | } | ||
241 | |||
242 | public void UpdateRHS(float timeStep) | ||
243 | { | ||
244 | } | ||
245 | } | ||
246 | } | ||