aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs312
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
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 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
generated by cgit v1.1 at 2024-09-03 05:42:46 +0000