aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs246
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
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace 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}
generated by cgit v1.1 at 2024-09-13 08:57:11 +0000