aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs310
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
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 /// 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}
generated by cgit v1.1 at 2024-08-22 13:51:12 +0000