diff options
Diffstat (limited to '')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs | 416 |
1 files changed, 208 insertions, 208 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs index 361c277..52084f4 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs | |||
@@ -1,208 +1,208 @@ | |||
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 | 27 | namespace XnaDevRu.BulletX |
28 | { | 28 | { |
29 | /// <summary> | 29 | /// <summary> |
30 | /// ConeShape implements a Cone shape, around the X axis | 30 | /// ConeShape implements a Cone shape, around the X axis |
31 | /// </summary> | 31 | /// </summary> |
32 | public class ConeShapeX : ConeShape | 32 | public class ConeShapeX : ConeShape |
33 | { | 33 | { |
34 | public ConeShapeX(float radius, float height) | 34 | public ConeShapeX(float radius, float height) |
35 | : base(radius, height) | 35 | : base(radius, height) |
36 | { | 36 | { |
37 | ConeUpIndex = 0; | 37 | ConeUpIndex = 0; |
38 | } | 38 | } |
39 | } | 39 | } |
40 | 40 | ||
41 | /// <summary> | 41 | /// <summary> |
42 | /// ConeShape implements a Cone shape, around the Z axis | 42 | /// ConeShape implements a Cone shape, around the Z axis |
43 | /// </summary> | 43 | /// </summary> |
44 | public class ConeShapeZ : ConeShape | 44 | public class ConeShapeZ : ConeShape |
45 | { | 45 | { |
46 | public ConeShapeZ(float radius, float height) | 46 | public ConeShapeZ(float radius, float height) |
47 | : base(radius, height) | 47 | : base(radius, height) |
48 | { | 48 | { |
49 | ConeUpIndex = 2; | 49 | ConeUpIndex = 2; |
50 | } | 50 | } |
51 | } | 51 | } |
52 | 52 | ||
53 | /// <summary> | 53 | /// <summary> |
54 | /// ConeShape implements a Cone shape, around the Y axis | 54 | /// ConeShape implements a Cone shape, around the Y axis |
55 | /// </summary> | 55 | /// </summary> |
56 | public class ConeShape : ConvexShape | 56 | public class ConeShape : ConvexShape |
57 | { | 57 | { |
58 | private float _sinAngle; | 58 | private float _sinAngle; |
59 | private float _radius; | 59 | private float _radius; |
60 | private float _height; | 60 | private float _height; |
61 | private int[] _coneIndices = new int[3]; | 61 | private int[] _coneIndices = new int[3]; |
62 | 62 | ||
63 | public ConeShape(float radius, float height) | 63 | public ConeShape(float radius, float height) |
64 | { | 64 | { |
65 | _radius = radius; | 65 | _radius = radius; |
66 | _height = height; | 66 | _height = height; |
67 | ConeUpIndex = 1; | 67 | ConeUpIndex = 1; |
68 | _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height)); | 68 | _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height)); |
69 | } | 69 | } |
70 | 70 | ||
71 | public float Radius { get { return _radius; } } | 71 | public float Radius { get { return _radius; } } |
72 | public float Height { get { return _height; } } | 72 | public float Height { get { return _height; } } |
73 | 73 | ||
74 | public override BroadphaseNativeTypes ShapeType | 74 | public override BroadphaseNativeTypes ShapeType |
75 | { | 75 | { |
76 | get | 76 | get |
77 | { | 77 | { |
78 | return BroadphaseNativeTypes.Cone; | 78 | return BroadphaseNativeTypes.Cone; |
79 | } | 79 | } |
80 | } | 80 | } |
81 | 81 | ||
82 | public override string Name | 82 | public override string Name |
83 | { | 83 | { |
84 | get | 84 | get |
85 | { | 85 | { |
86 | return "Cone"; | 86 | return "Cone"; |
87 | } | 87 | } |
88 | } | 88 | } |
89 | 89 | ||
90 | //choose upAxis index | 90 | //choose upAxis index |
91 | public int ConeUpIndex | 91 | public int ConeUpIndex |
92 | { | 92 | { |
93 | get { return _coneIndices[1]; } | 93 | get { return _coneIndices[1]; } |
94 | set | 94 | set |
95 | { | 95 | { |
96 | switch (value) | 96 | switch (value) |
97 | { | 97 | { |
98 | case 0: | 98 | case 0: |
99 | _coneIndices[0] = 1; | 99 | _coneIndices[0] = 1; |
100 | _coneIndices[1] = 0; | 100 | _coneIndices[1] = 0; |
101 | _coneIndices[2] = 2; | 101 | _coneIndices[2] = 2; |
102 | break; | 102 | break; |
103 | case 1: | 103 | case 1: |
104 | _coneIndices[0] = 0; | 104 | _coneIndices[0] = 0; |
105 | _coneIndices[1] = 1; | 105 | _coneIndices[1] = 1; |
106 | _coneIndices[2] = 2; | 106 | _coneIndices[2] = 2; |
107 | break; | 107 | break; |
108 | case 2: | 108 | case 2: |
109 | _coneIndices[0] = 0; | 109 | _coneIndices[0] = 0; |
110 | _coneIndices[1] = 2; | 110 | _coneIndices[1] = 2; |
111 | _coneIndices[2] = 1; | 111 | _coneIndices[2] = 1; |
112 | break; | 112 | break; |
113 | default: | 113 | default: |
114 | BulletDebug.Assert(false); | 114 | BulletDebug.Assert(false); |
115 | break; | 115 | break; |
116 | } | 116 | } |
117 | } | 117 | } |
118 | } | 118 | } |
119 | 119 | ||
120 | private Vector3 ConeLocalSupport(Vector3 v) | 120 | private Vector3 ConeLocalSupport(Vector3 v) |
121 | { | 121 | { |
122 | float halfHeight = _height * 0.5f; | 122 | float halfHeight = _height * 0.5f; |
123 | bool condition; | 123 | bool condition; |
124 | 124 | ||
125 | if (_coneIndices[1] == 0) | 125 | if (_coneIndices[1] == 0) |
126 | condition = v.X > v.Length() * _sinAngle; | 126 | condition = v.X > v.Length() * _sinAngle; |
127 | else if (_coneIndices[1] == 1) | 127 | else if (_coneIndices[1] == 1) |
128 | condition = v.Y > v.Length() * _sinAngle; | 128 | condition = v.Y > v.Length() * _sinAngle; |
129 | else | 129 | else |
130 | condition = v.Z > v.Length() * _sinAngle; | 130 | condition = v.Z > v.Length() * _sinAngle; |
131 | 131 | ||
132 | if (condition) | 132 | if (condition) |
133 | { | 133 | { |
134 | Vector3 tmp = new Vector3(); | 134 | Vector3 tmp = new Vector3(); |
135 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight); | 135 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight); |
136 | return tmp; | 136 | return tmp; |
137 | } | 137 | } |
138 | else | 138 | else |
139 | { | 139 | { |
140 | float s = (float)Math.Sqrt(MathHelper.GetValueByIndex(v, _coneIndices[0]) * MathHelper.GetValueByIndex(v, _coneIndices[0]) | 140 | float s = (float)Math.Sqrt(MathHelper.GetValueByIndex(v, _coneIndices[0]) * MathHelper.GetValueByIndex(v, _coneIndices[0]) |
141 | + MathHelper.GetValueByIndex(v, _coneIndices[2]) * MathHelper.GetValueByIndex(v, _coneIndices[2])); | 141 | + MathHelper.GetValueByIndex(v, _coneIndices[2]) * MathHelper.GetValueByIndex(v, _coneIndices[2])); |
142 | if (s > MathHelper.Epsilon) | 142 | if (s > MathHelper.Epsilon) |
143 | { | 143 | { |
144 | float d = _radius / s; | 144 | float d = _radius / s; |
145 | Vector3 tmp = new Vector3(); | 145 | Vector3 tmp = new Vector3(); |
146 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d); | 146 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d); |
147 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); | 147 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); |
148 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d); | 148 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d); |
149 | return tmp; | 149 | return tmp; |
150 | } | 150 | } |
151 | else | 151 | else |
152 | { | 152 | { |
153 | Vector3 tmp = new Vector3(); | 153 | Vector3 tmp = new Vector3(); |
154 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); | 154 | MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight); |
155 | return tmp; | 155 | return tmp; |
156 | } | 156 | } |
157 | } | 157 | } |
158 | } | 158 | } |
159 | 159 | ||
160 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) | 160 | public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec) |
161 | { | 161 | { |
162 | return ConeLocalSupport(vec); | 162 | return ConeLocalSupport(vec); |
163 | } | 163 | } |
164 | 164 | ||
165 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) | 165 | public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) |
166 | { | 166 | { |
167 | for (int i = 0; i < vectors.Length; i++) | 167 | for (int i = 0; i < vectors.Length; i++) |
168 | supportVerticesOut[i] = ConeLocalSupport(vectors[i]); | 168 | supportVerticesOut[i] = ConeLocalSupport(vectors[i]); |
169 | } | 169 | } |
170 | 170 | ||
171 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) | 171 | public override void CalculateLocalInertia(float mass, out Vector3 inertia) |
172 | { | 172 | { |
173 | Matrix identity = Matrix.Identity; | 173 | Matrix identity = Matrix.Identity; |
174 | Vector3 aabbMin, aabbMax; | 174 | Vector3 aabbMin, aabbMax; |
175 | GetAabb(identity, out aabbMin, out aabbMax); | 175 | GetAabb(identity, out aabbMin, out aabbMax); |
176 | 176 | ||
177 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; | 177 | Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f; |
178 | 178 | ||
179 | float margin = Margin; | 179 | float margin = Margin; |
180 | 180 | ||
181 | float lx = 2f * (halfExtents.X + margin); | 181 | float lx = 2f * (halfExtents.X + margin); |
182 | float ly = 2f * (halfExtents.Y + margin); | 182 | float ly = 2f * (halfExtents.Y + margin); |
183 | float lz = 2f * (halfExtents.Z + margin); | 183 | float lz = 2f * (halfExtents.Z + margin); |
184 | float x2 = lx * lx; | 184 | float x2 = lx * lx; |
185 | float y2 = ly * ly; | 185 | float y2 = ly * ly; |
186 | float z2 = lz * lz; | 186 | float z2 = lz * lz; |
187 | float scaledmass = mass * 0.08333333f; | 187 | float scaledmass = mass * 0.08333333f; |
188 | 188 | ||
189 | inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); | 189 | inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2)); |
190 | } | 190 | } |
191 | 191 | ||
192 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) | 192 | public override Vector3 LocalGetSupportingVertex(Vector3 vec) |
193 | { | 193 | { |
194 | Vector3 supVertex = ConeLocalSupport(vec); | 194 | Vector3 supVertex = ConeLocalSupport(vec); |
195 | if (Margin != 0) | 195 | if (Margin != 0) |
196 | { | 196 | { |
197 | Vector3 vecnorm = vec; | 197 | Vector3 vecnorm = vec; |
198 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) | 198 | if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon)) |
199 | { | 199 | { |
200 | vecnorm = new Vector3(-1f, -1f, -1f); | 200 | vecnorm = new Vector3(-1f, -1f, -1f); |
201 | } | 201 | } |
202 | vecnorm = Vector3.Normalize(vecnorm); | 202 | vecnorm = Vector3.Normalize(vecnorm); |
203 | supVertex += Margin * vecnorm; | 203 | supVertex += Margin * vecnorm; |
204 | } | 204 | } |
205 | return supVertex; | 205 | return supVertex; |
206 | } | 206 | } |
207 | } | 207 | } |
208 | } | 208 | } |