diff options
Diffstat (limited to 'libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs')
-rw-r--r-- | libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs | 378 |
1 files changed, 189 insertions, 189 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs index c4949f0..39438e4 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs | |||
@@ -1,189 +1,189 @@ | |||
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 | public class ConvexConcaveCollisionAlgorithm : CollisionAlgorithm | 29 | public class ConvexConcaveCollisionAlgorithm : CollisionAlgorithm |
30 | { | 30 | { |
31 | private bool _isSwapped; | 31 | private bool _isSwapped; |
32 | private ConvexTriangleCallback _convexTriangleCallback; | 32 | private ConvexTriangleCallback _convexTriangleCallback; |
33 | 33 | ||
34 | public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) | 34 | public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped) |
35 | : base(collisionAlgorithmConstructionInfo) | 35 | : base(collisionAlgorithmConstructionInfo) |
36 | { | 36 | { |
37 | _isSwapped = isSwapped; | 37 | _isSwapped = isSwapped; |
38 | _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped); | 38 | _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped); |
39 | } | 39 | } |
40 | 40 | ||
41 | public void ClearCache() | 41 | public void ClearCache() |
42 | { | 42 | { |
43 | _convexTriangleCallback.ClearCache(); | 43 | _convexTriangleCallback.ClearCache(); |
44 | } | 44 | } |
45 | 45 | ||
46 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 46 | public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
47 | { | 47 | { |
48 | CollisionObject convexBody = _isSwapped ? bodyB : bodyA; | 48 | CollisionObject convexBody = _isSwapped ? bodyB : bodyA; |
49 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; | 49 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; |
50 | 50 | ||
51 | if (triBody.CollisionShape.IsConcave) | 51 | if (triBody.CollisionShape.IsConcave) |
52 | { | 52 | { |
53 | CollisionObject triOb = triBody; | 53 | CollisionObject triOb = triBody; |
54 | ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape; | 54 | ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape; |
55 | 55 | ||
56 | if (convexBody.CollisionShape.IsConvex) | 56 | if (convexBody.CollisionShape.IsConvex) |
57 | { | 57 | { |
58 | float collisionMarginTriangle = concaveShape.Margin; | 58 | float collisionMarginTriangle = concaveShape.Margin; |
59 | 59 | ||
60 | resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold); | 60 | resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold); |
61 | _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); | 61 | _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); |
62 | 62 | ||
63 | //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. | 63 | //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. |
64 | //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr); | 64 | //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr); |
65 | 65 | ||
66 | _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody); | 66 | _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody); |
67 | concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax); | 67 | concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax); |
68 | } | 68 | } |
69 | } | 69 | } |
70 | } | 70 | } |
71 | 71 | ||
72 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) | 72 | public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) |
73 | { | 73 | { |
74 | CollisionObject convexbody = _isSwapped ? bodyB : bodyA; | 74 | CollisionObject convexbody = _isSwapped ? bodyB : bodyA; |
75 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; | 75 | CollisionObject triBody = _isSwapped ? bodyA : bodyB; |
76 | 76 | ||
77 | 77 | ||
78 | //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) | 78 | //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) |
79 | 79 | ||
80 | //only perform CCD above a certain threshold, this prevents blocking on the long run | 80 | //only perform CCD above a certain threshold, this prevents blocking on the long run |
81 | //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... | 81 | //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... |
82 | float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared(); | 82 | float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared(); |
83 | if (squareMot0 < convexbody.CcdSquareMotionThreshold) | 83 | if (squareMot0 < convexbody.CcdSquareMotionThreshold) |
84 | { | 84 | { |
85 | return 1; | 85 | return 1; |
86 | } | 86 | } |
87 | 87 | ||
88 | Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform); | 88 | Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform); |
89 | Matrix convexFromLocal = triInv * convexbody.WorldTransform; | 89 | Matrix convexFromLocal = triInv * convexbody.WorldTransform; |
90 | Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform; | 90 | Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform; |
91 | 91 | ||
92 | if (triBody.CollisionShape.IsConcave) | 92 | if (triBody.CollisionShape.IsConcave) |
93 | { | 93 | { |
94 | Vector3 rayAabbMin = convexFromLocal.Translation; | 94 | Vector3 rayAabbMin = convexFromLocal.Translation; |
95 | MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation); | 95 | MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation); |
96 | Vector3 rayAabbMax = convexFromLocal.Translation; | 96 | Vector3 rayAabbMax = convexFromLocal.Translation; |
97 | MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation); | 97 | MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation); |
98 | float ccdRadius0 = convexbody.CcdSweptSphereRadius; | 98 | float ccdRadius0 = convexbody.CcdSweptSphereRadius; |
99 | rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); | 99 | rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); |
100 | rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); | 100 | rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0); |
101 | 101 | ||
102 | float curHitFraction = 1f; //is this available? | 102 | float curHitFraction = 1f; //is this available? |
103 | LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, | 103 | LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, |
104 | convexbody.CcdSweptSphereRadius, curHitFraction); | 104 | convexbody.CcdSweptSphereRadius, curHitFraction); |
105 | 105 | ||
106 | raycastCallback.HitFraction = convexbody.HitFraction; | 106 | raycastCallback.HitFraction = convexbody.HitFraction; |
107 | 107 | ||
108 | CollisionObject concavebody = triBody; | 108 | CollisionObject concavebody = triBody; |
109 | 109 | ||
110 | ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape; | 110 | ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape; |
111 | 111 | ||
112 | if (triangleMesh != null) | 112 | if (triangleMesh != null) |
113 | { | 113 | { |
114 | triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); | 114 | triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); |
115 | } | 115 | } |
116 | 116 | ||
117 | if (raycastCallback.HitFraction < convexbody.HitFraction) | 117 | if (raycastCallback.HitFraction < convexbody.HitFraction) |
118 | { | 118 | { |
119 | convexbody.HitFraction = raycastCallback.HitFraction; | 119 | convexbody.HitFraction = raycastCallback.HitFraction; |
120 | return raycastCallback.HitFraction; | 120 | return raycastCallback.HitFraction; |
121 | } | 121 | } |
122 | } | 122 | } |
123 | 123 | ||
124 | return 1; | 124 | return 1; |
125 | } | 125 | } |
126 | 126 | ||
127 | public class CreateFunc : CollisionAlgorithmCreateFunction | 127 | public class CreateFunc : CollisionAlgorithmCreateFunction |
128 | { | 128 | { |
129 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 129 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
130 | { | 130 | { |
131 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); | 131 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false); |
132 | } | 132 | } |
133 | } | 133 | } |
134 | 134 | ||
135 | public class SwappedCreateFunc : CollisionAlgorithmCreateFunction | 135 | public class SwappedCreateFunc : CollisionAlgorithmCreateFunction |
136 | { | 136 | { |
137 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) | 137 | public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB) |
138 | { | 138 | { |
139 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); | 139 | return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true); |
140 | } | 140 | } |
141 | } | 141 | } |
142 | 142 | ||
143 | private class LocalTriangleSphereCastCallback : ITriangleCallback | 143 | private class LocalTriangleSphereCastCallback : ITriangleCallback |
144 | { | 144 | { |
145 | private Matrix _ccdSphereFromTrans; | 145 | private Matrix _ccdSphereFromTrans; |
146 | private Matrix _ccdSphereToTrans; | 146 | private Matrix _ccdSphereToTrans; |
147 | private Matrix _meshTransform; | 147 | private Matrix _meshTransform; |
148 | 148 | ||
149 | private float _ccdSphereRadius; | 149 | private float _ccdSphereRadius; |
150 | private float _hitFraction; | 150 | private float _hitFraction; |
151 | 151 | ||
152 | public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction) | 152 | public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction) |
153 | { | 153 | { |
154 | _ccdSphereFromTrans = from; | 154 | _ccdSphereFromTrans = from; |
155 | _ccdSphereToTrans = to; | 155 | _ccdSphereToTrans = to; |
156 | _ccdSphereRadius = ccdSphereRadius; | 156 | _ccdSphereRadius = ccdSphereRadius; |
157 | _hitFraction = hitFraction; | 157 | _hitFraction = hitFraction; |
158 | } | 158 | } |
159 | 159 | ||
160 | public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } } | 160 | public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } } |
161 | public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } } | 161 | public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } } |
162 | public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } } | 162 | public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } } |
163 | public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } } | 163 | public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } } |
164 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } | 164 | public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } } |
165 | 165 | ||
166 | public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex) | 166 | public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex) |
167 | { | 167 | { |
168 | //do a swept sphere for now | 168 | //do a swept sphere for now |
169 | Matrix ident = Matrix.Identity; | 169 | Matrix ident = Matrix.Identity; |
170 | CastResult castResult = new CastResult(); | 170 | CastResult castResult = new CastResult(); |
171 | castResult.Fraction = _hitFraction; | 171 | castResult.Fraction = _hitFraction; |
172 | SphereShape pointShape = new SphereShape(_ccdSphereRadius); | 172 | SphereShape pointShape = new SphereShape(_ccdSphereRadius); |
173 | TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); | 173 | TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); |
174 | VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); | 174 | VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); |
175 | SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver); | 175 | SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver); |
176 | //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); | 176 | //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); |
177 | //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); | 177 | //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); |
178 | //local space? | 178 | //local space? |
179 | 179 | ||
180 | if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans, | 180 | if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans, |
181 | ident, ident, castResult)) | 181 | ident, ident, castResult)) |
182 | { | 182 | { |
183 | if (_hitFraction > castResult.Fraction) | 183 | if (_hitFraction > castResult.Fraction) |
184 | _hitFraction = castResult.Fraction; | 184 | _hitFraction = castResult.Fraction; |
185 | } | 185 | } |
186 | } | 186 | } |
187 | } | 187 | } |
188 | } | 188 | } |
189 | } | 189 | } |