aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs308
1 files changed, 154 insertions, 154 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
index 9959c01..73fd094 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
@@ -1,154 +1,154 @@
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 27namespace XnaDevRu.BulletX
28{ 28{
29 /// <summary> 29 /// <summary>
30 /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex) 30 /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex)
31 /// </summary> 31 /// </summary>
32 public class MultiSphereShape : ConvexShape 32 public class MultiSphereShape : ConvexShape
33 { 33 {
34 private const int _maxNumSpheres = 5; 34 private const int _maxNumSpheres = 5;
35 private Vector3[] _localPositions = new Vector3[MaxNumSpheres]; 35 private Vector3[] _localPositions = new Vector3[MaxNumSpheres];
36 private float[] _radi = new float[MaxNumSpheres]; 36 private float[] _radi = new float[MaxNumSpheres];
37 private Vector3 _inertiaHalfExtents; 37 private Vector3 _inertiaHalfExtents;
38 38
39 private int m_numSpheres; 39 private int m_numSpheres;
40 40
41 public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres) 41 public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres)
42 { 42 {
43 _inertiaHalfExtents = inertiaHalfExtents; 43 _inertiaHalfExtents = inertiaHalfExtents;
44 float startMargin = 1e30f; 44 float startMargin = 1e30f;
45 45
46 m_numSpheres = numSpheres; 46 m_numSpheres = numSpheres;
47 for (int i = 0; i < m_numSpheres; i++) 47 for (int i = 0; i < m_numSpheres; i++)
48 { 48 {
49 _localPositions[i] = positions[i]; 49 _localPositions[i] = positions[i];
50 _radi[i] = radi[i]; 50 _radi[i] = radi[i];
51 if (radi[i] < startMargin) 51 if (radi[i] < startMargin)
52 startMargin = radi[i]; 52 startMargin = radi[i];
53 } 53 }
54 Margin = startMargin; 54 Margin = startMargin;
55 } 55 }
56 56
57 public static int MaxNumSpheres { get { return _maxNumSpheres; } } 57 public static int MaxNumSpheres { get { return _maxNumSpheres; } }
58 58
59 public override BroadphaseNativeTypes ShapeType 59 public override BroadphaseNativeTypes ShapeType
60 { 60 {
61 get 61 get
62 { 62 {
63 return BroadphaseNativeTypes.MultiSphere; 63 return BroadphaseNativeTypes.MultiSphere;
64 } 64 }
65 } 65 }
66 66
67 public override string Name 67 public override string Name
68 { 68 {
69 get 69 get
70 { 70 {
71 return "MultiSphere"; 71 return "MultiSphere";
72 } 72 }
73 } 73 }
74 74
75 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA) 75 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA)
76 { 76 {
77 Vector3 supVec = new Vector3(); 77 Vector3 supVec = new Vector3();
78 78
79 float maxDot = -1e30f; 79 float maxDot = -1e30f;
80 80
81 81
82 Vector3 vec = vecA; 82 Vector3 vec = vecA;
83 float lenSqr = vec.LengthSquared(); 83 float lenSqr = vec.LengthSquared();
84 if (lenSqr < 0.0001f) 84 if (lenSqr < 0.0001f)
85 { 85 {
86 vec = new Vector3(1, 0, 0); 86 vec = new Vector3(1, 0, 0);
87 } 87 }
88 else 88 else
89 { 89 {
90 float rlen = 1f / (float)Math.Sqrt(lenSqr); 90 float rlen = 1f / (float)Math.Sqrt(lenSqr);
91 vec *= rlen; 91 vec *= rlen;
92 } 92 }
93 93
94 Vector3 vtx; 94 Vector3 vtx;
95 float newDot; 95 float newDot;
96 96
97 for (int i = 0; i < m_numSpheres; i++) 97 for (int i = 0; i < m_numSpheres; i++)
98 { 98 {
99 vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin; 99 vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin;
100 newDot = Vector3.Dot(vec, vtx); 100 newDot = Vector3.Dot(vec, vtx);
101 if (newDot > maxDot) 101 if (newDot > maxDot)
102 { 102 {
103 maxDot = newDot; 103 maxDot = newDot;
104 supVec = vtx; 104 supVec = vtx;
105 } 105 }
106 } 106 }
107 107
108 return supVec; 108 return supVec;
109 } 109 }
110 110
111 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut) 111 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
112 { 112 {
113 for (int j = 0; j < vectors.Length; j++) 113 for (int j = 0; j < vectors.Length; j++)
114 { 114 {
115 float maxDot = -1e30f; 115 float maxDot = -1e30f;
116 Vector3 vtx; 116 Vector3 vtx;
117 float newDot; 117 float newDot;
118 118
119 for (int i = 0; i < m_numSpheres; i++) 119 for (int i = 0; i < m_numSpheres; i++)
120 { 120 {
121 vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin; 121 vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin;
122 newDot = Vector3.Dot(vectors[j], vtx); 122 newDot = Vector3.Dot(vectors[j], vtx);
123 if (newDot > maxDot) 123 if (newDot > maxDot)
124 { 124 {
125 maxDot = newDot; 125 maxDot = newDot;
126 supportVerticesOut[j] = vtx; 126 supportVerticesOut[j] = vtx;
127 } 127 }
128 } 128 }
129 } 129 }
130 } 130 }
131 131
132 public override void CalculateLocalInertia(float mass, out Vector3 inertia) 132 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
133 { 133 {
134 //as an approximation, take the inertia of the box that bounds the spheres 134 //as an approximation, take the inertia of the box that bounds the spheres
135 Matrix ident = Matrix.Identity; 135 Matrix ident = Matrix.Identity;
136 Vector3 halfExtents = _inertiaHalfExtents; 136 Vector3 halfExtents = _inertiaHalfExtents;
137 137
138 float margin = ConvexDistanceMargin; 138 float margin = ConvexDistanceMargin;
139 139
140 float lx = 2f * (halfExtents.X + margin); 140 float lx = 2f * (halfExtents.X + margin);
141 float ly = 2f * (halfExtents.Y + margin); 141 float ly = 2f * (halfExtents.Y + margin);
142 float lz = 2f * (halfExtents.Z + margin); 142 float lz = 2f * (halfExtents.Z + margin);
143 float x2 = lx * lx; 143 float x2 = lx * lx;
144 float y2 = ly * ly; 144 float y2 = ly * ly;
145 float z2 = lz * lz; 145 float z2 = lz * lz;
146 float scaledmass = mass * 0.08333333f; 146 float scaledmass = mass * 0.08333333f;
147 147
148 inertia = new Vector3(); 148 inertia = new Vector3();
149 inertia.X = scaledmass * (y2 + z2); 149 inertia.X = scaledmass * (y2 + z2);
150 inertia.Y = scaledmass * (x2 + z2); 150 inertia.Y = scaledmass * (x2 + z2);
151 inertia.Z = scaledmass * (x2 + y2); 151 inertia.Z = scaledmass * (x2 + y2);
152 } 152 }
153 } 153 }
154} 154}
generated by cgit v1.1 at 2024-09-13 08:18:42 +0000