diff options
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs | 310 |
1 files changed, 155 insertions, 155 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs index 3b8d8d8..6249998 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs | |||
@@ -1,155 +1,155 @@ | |||
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 | /// <summary> | 29 | /// <summary> |
30 | /// Jacobian entry is an abstraction that allows to describe constraints | 30 | /// Jacobian entry is an abstraction that allows to describe constraints |
31 | /// it can be used in combination with a constraint solver | 31 | /// it can be used in combination with a constraint solver |
32 | /// Can be used to relate the effect of an impulse to the constraint error | 32 | /// Can be used to relate the effect of an impulse to the constraint error |
33 | /// </summary> | 33 | /// </summary> |
34 | public class JacobianEntry | 34 | public class JacobianEntry |
35 | { | 35 | { |
36 | private Vector3 _linearJointAxis; | 36 | private Vector3 _linearJointAxis; |
37 | private Vector3 _aJ; | 37 | private Vector3 _aJ; |
38 | private Vector3 _bJ; | 38 | private Vector3 _bJ; |
39 | private Vector3 _0MinvJt; | 39 | private Vector3 _0MinvJt; |
40 | private Vector3 _1MinvJt; | 40 | private Vector3 _1MinvJt; |
41 | private float _adiag; | 41 | private float _adiag; |
42 | 42 | ||
43 | public JacobianEntry() { } | 43 | public JacobianEntry() { } |
44 | 44 | ||
45 | //constraint between two different rigidbodies | 45 | //constraint between two different rigidbodies |
46 | public JacobianEntry( | 46 | public JacobianEntry( |
47 | Matrix world2A, | 47 | Matrix world2A, |
48 | Matrix world2B, | 48 | Matrix world2B, |
49 | Vector3 relPosA, Vector3 relPosB, | 49 | Vector3 relPosA, Vector3 relPosB, |
50 | Vector3 jointAxis, | 50 | Vector3 jointAxis, |
51 | Vector3 inertiaInvA, | 51 | Vector3 inertiaInvA, |
52 | float massInvA, | 52 | float massInvA, |
53 | Vector3 inertiaInvB, | 53 | Vector3 inertiaInvB, |
54 | float massInvB) | 54 | float massInvB) |
55 | { | 55 | { |
56 | _linearJointAxis = jointAxis; | 56 | _linearJointAxis = jointAxis; |
57 | _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A); | 57 | _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A); |
58 | _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B); | 58 | _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B); |
59 | _0MinvJt = inertiaInvA * _aJ; | 59 | _0MinvJt = inertiaInvA * _aJ; |
60 | _1MinvJt = inertiaInvB * _bJ; | 60 | _1MinvJt = inertiaInvB * _bJ; |
61 | _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ); | 61 | _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ); |
62 | 62 | ||
63 | if (_adiag <= 0.0f) | 63 | if (_adiag <= 0.0f) |
64 | throw new BulletException(); | 64 | throw new BulletException(); |
65 | } | 65 | } |
66 | 66 | ||
67 | //angular constraint between two different rigidbodies | 67 | //angular constraint between two different rigidbodies |
68 | public JacobianEntry(Vector3 jointAxis, | 68 | public JacobianEntry(Vector3 jointAxis, |
69 | Matrix world2A, | 69 | Matrix world2A, |
70 | Matrix world2B, | 70 | Matrix world2B, |
71 | Vector3 inertiaInvA, | 71 | Vector3 inertiaInvA, |
72 | Vector3 inertiaInvB) | 72 | Vector3 inertiaInvB) |
73 | { | 73 | { |
74 | _linearJointAxis = new Vector3(); | 74 | _linearJointAxis = new Vector3(); |
75 | _aJ = Vector3.TransformNormal(jointAxis, world2A); | 75 | _aJ = Vector3.TransformNormal(jointAxis, world2A); |
76 | _bJ = Vector3.TransformNormal(-jointAxis, world2B); | 76 | _bJ = Vector3.TransformNormal(-jointAxis, world2B); |
77 | _0MinvJt = inertiaInvA * _aJ; | 77 | _0MinvJt = inertiaInvA * _aJ; |
78 | _1MinvJt = inertiaInvB * _bJ; | 78 | _1MinvJt = inertiaInvB * _bJ; |
79 | _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); | 79 | _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); |
80 | 80 | ||
81 | if (_adiag <= 0.0f) | 81 | if (_adiag <= 0.0f) |
82 | throw new BulletException(); | 82 | throw new BulletException(); |
83 | } | 83 | } |
84 | 84 | ||
85 | //angular constraint between two different rigidbodies | 85 | //angular constraint between two different rigidbodies |
86 | public JacobianEntry(Vector3 axisInA, | 86 | public JacobianEntry(Vector3 axisInA, |
87 | Vector3 axisInB, | 87 | Vector3 axisInB, |
88 | Vector3 inertiaInvA, | 88 | Vector3 inertiaInvA, |
89 | Vector3 inertiaInvB) | 89 | Vector3 inertiaInvB) |
90 | { | 90 | { |
91 | _linearJointAxis = new Vector3(); | 91 | _linearJointAxis = new Vector3(); |
92 | _aJ = axisInA; | 92 | _aJ = axisInA; |
93 | _bJ = -axisInB; | 93 | _bJ = -axisInB; |
94 | _0MinvJt = inertiaInvA * _aJ; | 94 | _0MinvJt = inertiaInvA * _aJ; |
95 | _1MinvJt = inertiaInvB * _bJ; | 95 | _1MinvJt = inertiaInvB * _bJ; |
96 | _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); | 96 | _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ); |
97 | 97 | ||
98 | if (_adiag <= 0.0f) | 98 | if (_adiag <= 0.0f) |
99 | throw new BulletException(); | 99 | throw new BulletException(); |
100 | } | 100 | } |
101 | 101 | ||
102 | //constraint on one rigidbody | 102 | //constraint on one rigidbody |
103 | public JacobianEntry( | 103 | public JacobianEntry( |
104 | Matrix world2A, | 104 | Matrix world2A, |
105 | Vector3 rel_pos1, Vector3 rel_pos2, | 105 | Vector3 rel_pos1, Vector3 rel_pos2, |
106 | Vector3 jointAxis, | 106 | Vector3 jointAxis, |
107 | Vector3 inertiaInvA, | 107 | Vector3 inertiaInvA, |
108 | float massInvA) | 108 | float massInvA) |
109 | { | 109 | { |
110 | _linearJointAxis = jointAxis; | 110 | _linearJointAxis = jointAxis; |
111 | _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A); | 111 | _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A); |
112 | _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A); | 112 | _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A); |
113 | _0MinvJt = inertiaInvA * _aJ; | 113 | _0MinvJt = inertiaInvA * _aJ; |
114 | _1MinvJt = new Vector3(); | 114 | _1MinvJt = new Vector3(); |
115 | _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ); | 115 | _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ); |
116 | 116 | ||
117 | if (_adiag <= 0.0f) | 117 | if (_adiag <= 0.0f) |
118 | throw new BulletException(); | 118 | throw new BulletException(); |
119 | } | 119 | } |
120 | 120 | ||
121 | public float Diagonal { get { return _adiag; } } | 121 | public float Diagonal { get { return _adiag; } } |
122 | 122 | ||
123 | // for two constraints on the same rigidbody (for example vehicle friction) | 123 | // for two constraints on the same rigidbody (for example vehicle friction) |
124 | public float GetNonDiagonal(JacobianEntry jacB, float massInvA) | 124 | public float GetNonDiagonal(JacobianEntry jacB, float massInvA) |
125 | { | 125 | { |
126 | float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis); | 126 | float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis); |
127 | float ang = Vector3.Dot(_0MinvJt, jacB._aJ); | 127 | float ang = Vector3.Dot(_0MinvJt, jacB._aJ); |
128 | return lin + ang; | 128 | return lin + ang; |
129 | } | 129 | } |
130 | 130 | ||
131 | // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) | 131 | // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies) |
132 | public float GetNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB) | 132 | public float GetNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB) |
133 | { | 133 | { |
134 | Vector3 lin = _linearJointAxis * jacB._linearJointAxis; | 134 | Vector3 lin = _linearJointAxis * jacB._linearJointAxis; |
135 | Vector3 ang0 = _0MinvJt * jacB._aJ; | 135 | Vector3 ang0 = _0MinvJt * jacB._aJ; |
136 | Vector3 ang1 = _1MinvJt * jacB._bJ; | 136 | Vector3 ang1 = _1MinvJt * jacB._bJ; |
137 | Vector3 lin0 = massInvA * lin; | 137 | Vector3 lin0 = massInvA * lin; |
138 | Vector3 lin1 = massInvB * lin; | 138 | Vector3 lin1 = massInvB * lin; |
139 | Vector3 sum = ang0 + ang1 + lin0 + lin1; | 139 | Vector3 sum = ang0 + ang1 + lin0 + lin1; |
140 | return sum.X + sum.Y + sum.Z; | 140 | return sum.X + sum.Y + sum.Z; |
141 | } | 141 | } |
142 | 142 | ||
143 | public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB) | 143 | public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB) |
144 | { | 144 | { |
145 | Vector3 linrel = linvelA - linvelB; | 145 | Vector3 linrel = linvelA - linvelB; |
146 | Vector3 angvela = angvelA * _aJ; | 146 | Vector3 angvela = angvelA * _aJ; |
147 | Vector3 angvelb = angvelB * _bJ; | 147 | Vector3 angvelb = angvelB * _bJ; |
148 | linrel *= _linearJointAxis; | 148 | linrel *= _linearJointAxis; |
149 | angvela += angvelb; | 149 | angvela += angvelb; |
150 | angvela += linrel; | 150 | angvela += linrel; |
151 | float rel_vel2 = angvela.X + angvela.Y + angvela.Z; | 151 | float rel_vel2 = angvela.X + angvela.Y + angvela.Z; |
152 | return rel_vel2 + float.Epsilon; | 152 | return rel_vel2 + float.Epsilon; |
153 | } | 153 | } |
154 | } | 154 | } |
155 | } | 155 | } |