diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs | 312 |
1 files changed, 156 insertions, 156 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs index d4206ea..8ffd3d5 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs | |||
@@ -1,157 +1,157 @@ | |||
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 | public class ConstraintSetting | 29 | public class ConstraintSetting |
30 | { | 30 | { |
31 | private float _tau, _damping; | 31 | private float _tau, _damping; |
32 | 32 | ||
33 | public ConstraintSetting() | 33 | public ConstraintSetting() |
34 | { | 34 | { |
35 | _tau = 0.3f; | 35 | _tau = 0.3f; |
36 | _damping = 1.0f; | 36 | _damping = 1.0f; |
37 | } | 37 | } |
38 | 38 | ||
39 | public float Damping | 39 | public float Damping |
40 | { | 40 | { |
41 | get { return _damping; } | 41 | get { return _damping; } |
42 | set { _damping = value; } | 42 | set { _damping = value; } |
43 | } | 43 | } |
44 | 44 | ||
45 | public float Tau | 45 | public float Tau |
46 | { | 46 | { |
47 | get { return _tau; } | 47 | get { return _tau; } |
48 | set { _tau = value; } | 48 | set { _tau = value; } |
49 | } | 49 | } |
50 | } | 50 | } |
51 | 51 | ||
52 | public class Point2PointConstraint : TypedConstraint | 52 | public class Point2PointConstraint : TypedConstraint |
53 | { | 53 | { |
54 | private JacobianEntry[] _jacobian; | 54 | private JacobianEntry[] _jacobian; |
55 | private Vector3 _pivotInA, _pivotInB; | 55 | private Vector3 _pivotInA, _pivotInB; |
56 | 56 | ||
57 | private ConstraintSetting _setting = new ConstraintSetting(); | 57 | private ConstraintSetting _setting = new ConstraintSetting(); |
58 | 58 | ||
59 | public Point2PointConstraint() | 59 | public Point2PointConstraint() |
60 | { | 60 | { |
61 | _jacobian = new JacobianEntry[3]; | 61 | _jacobian = new JacobianEntry[3]; |
62 | } | 62 | } |
63 | 63 | ||
64 | public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB) | 64 | public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB) |
65 | : base(rbA, rbB) | 65 | : base(rbA, rbB) |
66 | { | 66 | { |
67 | _jacobian = new JacobianEntry[3]; | 67 | _jacobian = new JacobianEntry[3]; |
68 | 68 | ||
69 | _pivotInA = pivotInA; | 69 | _pivotInA = pivotInA; |
70 | _pivotInB = pivotInB; | 70 | _pivotInB = pivotInB; |
71 | } | 71 | } |
72 | 72 | ||
73 | public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA) | 73 | public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA) |
74 | : base(rbA) | 74 | : base(rbA) |
75 | { | 75 | { |
76 | _jacobian = new JacobianEntry[3]; | 76 | _jacobian = new JacobianEntry[3]; |
77 | 77 | ||
78 | _pivotInA = pivotInA; | 78 | _pivotInA = pivotInA; |
79 | _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA); | 79 | _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA); |
80 | } | 80 | } |
81 | 81 | ||
82 | public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } } | 82 | public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } } |
83 | 83 | ||
84 | public Vector3 PivotInA | 84 | public Vector3 PivotInA |
85 | { | 85 | { |
86 | set | 86 | set |
87 | { | 87 | { |
88 | _pivotInA = value; | 88 | _pivotInA = value; |
89 | } | 89 | } |
90 | } | 90 | } |
91 | 91 | ||
92 | public Vector3 PivotInB | 92 | public Vector3 PivotInB |
93 | { | 93 | { |
94 | set | 94 | set |
95 | { | 95 | { |
96 | _pivotInB = value; | 96 | _pivotInB = value; |
97 | } | 97 | } |
98 | } | 98 | } |
99 | 99 | ||
100 | public override void BuildJacobian() | 100 | public override void BuildJacobian() |
101 | { | 101 | { |
102 | Vector3 normal = new Vector3(); | 102 | Vector3 normal = new Vector3(); |
103 | 103 | ||
104 | for (int i = 0; i < 3; i++) | 104 | for (int i = 0; i < 3; i++) |
105 | { | 105 | { |
106 | MathHelper.SetElement(ref normal, i, 1); | 106 | MathHelper.SetElement(ref normal, i, 1); |
107 | _jacobian[i] = new JacobianEntry( | 107 | _jacobian[i] = new JacobianEntry( |
108 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), | 108 | MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform), |
109 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), | 109 | MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform), |
110 | MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, | 110 | MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition, |
111 | MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, | 111 | MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition, |
112 | normal, | 112 | normal, |
113 | RigidBodyA.InvInertiaDiagLocal, | 113 | RigidBodyA.InvInertiaDiagLocal, |
114 | RigidBodyA.InverseMass, | 114 | RigidBodyA.InverseMass, |
115 | RigidBodyB.InvInertiaDiagLocal, | 115 | RigidBodyB.InvInertiaDiagLocal, |
116 | RigidBodyB.InverseMass | 116 | RigidBodyB.InverseMass |
117 | ); | 117 | ); |
118 | MathHelper.SetElement(ref normal, i, 0); | 118 | MathHelper.SetElement(ref normal, i, 0); |
119 | } | 119 | } |
120 | } | 120 | } |
121 | 121 | ||
122 | public override void SolveConstraint(float timeStep) | 122 | public override void SolveConstraint(float timeStep) |
123 | { | 123 | { |
124 | Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); | 124 | Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform); |
125 | Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); | 125 | Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform); |
126 | 126 | ||
127 | Vector3 normal = new Vector3(); | 127 | Vector3 normal = new Vector3(); |
128 | 128 | ||
129 | for (int i = 0; i < 3; i++) | 129 | for (int i = 0; i < 3; i++) |
130 | { | 130 | { |
131 | MathHelper.SetElement(ref normal, i, 1); | 131 | MathHelper.SetElement(ref normal, i, 1); |
132 | 132 | ||
133 | float jacDiagABInv = 1.0f / _jacobian[i].Diagonal; | 133 | float jacDiagABInv = 1.0f / _jacobian[i].Diagonal; |
134 | 134 | ||
135 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; | 135 | Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition; |
136 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; | 136 | Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition; |
137 | 137 | ||
138 | Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); | 138 | Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1); |
139 | Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); | 139 | Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2); |
140 | 140 | ||
141 | Vector3 vel = vel1 - vel2; | 141 | Vector3 vel = vel1 - vel2; |
142 | 142 | ||
143 | float rel_vel = Vector3.Dot(normal, vel); | 143 | float rel_vel = Vector3.Dot(normal, vel); |
144 | float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal); | 144 | float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal); |
145 | 145 | ||
146 | float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv; | 146 | float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv; |
147 | AppliedImpulse += impulse; | 147 | AppliedImpulse += impulse; |
148 | Vector3 impulseVector = normal * impulse; | 148 | Vector3 impulseVector = normal * impulse; |
149 | 149 | ||
150 | RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition); | 150 | RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition); |
151 | RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition); | 151 | RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition); |
152 | 152 | ||
153 | MathHelper.SetElement(ref normal, i, 0); | 153 | MathHelper.SetElement(ref normal, i, 0); |
154 | } | 154 | } |
155 | } | 155 | } |
156 | } | 156 | } |
157 | } \ No newline at end of file | 157 | } \ No newline at end of file |