← Back to branch summary

~ubuntu-branches/ubuntu/precise/supertuxkart/precise

« back to all changes in this revision

Viewing changes to src/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Christoph Egger
  • Date: 2011-02-24 22:36:25 UTC
  • mfrom: (1.1.9 upstream) (6.1.4 sid)
  • Revision ID: james.westby@ubuntu.com-20110224223625-ygrjfpg92obovuch
Tags: 0.7+dfsg1-1
New upstream release

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/bullet/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
1
 
/*
2
 
Bullet Continuous Collision Detection and Physics Library
3
 
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
 
 
5
 
This software is provided 'as-is', without any express or implied warranty.
6
 
In no event will the authors be held liable for any damages arising from the use of this software.
7
 
Permission is granted to anyone to use this software for any purpose, 
8
 
including commercial applications, and to alter it and redistribute it freely, 
9
 
subject to the following restrictions:
10
 
 
11
 
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12
 
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13
 
3. This notice may not be removed or altered from any source distribution.
14
 
*/
15
 
 
16
 
#include "LinearMath/btScalar.h"
17
 
#include "SphereTriangleDetector.h"
18
 
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
19
 
#include "BulletCollision/CollisionShapes/btSphereShape.h"
20
 
 
21
 
 
22
 
SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle)
23
 
:m_sphere(sphere),
24
 
m_triangle(triangle)
25
 
{
26
 
 
27
 
}
28
 
 
29
 
void    SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
30
 
{
31
 
 
32
 
        (void)debugDraw;
33
 
        const btTransform& transformA = input.m_transformA;
34
 
        const btTransform& transformB = input.m_transformB;
35
 
 
36
 
        btVector3 point,normal;
37
 
        btScalar timeOfImpact = btScalar(1.);
38
 
        btScalar depth = btScalar(0.);
39
 
//      output.m_distance = btScalar(1e30);
40
 
        //move sphere into triangle space
41
 
        btTransform     sphereInTr = transformB.inverseTimes(transformA);
42
 
 
43
 
        if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact))
44
 
        {
45
 
                output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
46
 
        }
47
 
 
48
 
}
49
 
 
50
 
#define MAX_OVERLAP btScalar(0.)
51
 
 
52
 
 
53
 
 
54
 
// See also geometrictools.com
55
 
// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
56
 
btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
57
 
        btVector3 diff = p - from;
58
 
        btVector3 v = to - from;
59
 
        btScalar t = v.dot(diff);
60
 
        
61
 
        if (t > 0) {
62
 
                btScalar dotVV = v.dot(v);
63
 
                if (t < dotVV) {
64
 
                        t /= dotVV;
65
 
                        diff -= t*v;
66
 
                } else {
67
 
                        t = 1;
68
 
                        diff -= v;
69
 
                }
70
 
        } else
71
 
                t = 0;
72
 
 
73
 
        nearest = from + t*v;
74
 
        return diff.dot(diff);  
75
 
}
76
 
 
77
 
bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
78
 
        btVector3 lp(p);
79
 
        btVector3 lnormal(normal);
80
 
        
81
 
        return pointInTriangle(vertices, lnormal, &lp);
82
 
}
83
 
 
84
 
///combined discrete/continuous sphere-triangle
85
 
bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact)
86
 
{
87
 
 
88
 
        const btVector3* vertices = &m_triangle->getVertexPtr(0);
89
 
        const btVector3& c = sphereCenter;
90
 
        btScalar r = m_sphere->getRadius();
91
 
 
92
 
        btVector3 delta (0,0,0);
93
 
 
94
 
        btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
95
 
        normal.normalize();
96
 
        btVector3 p1ToCentre = c - vertices[0];
97
 
        btScalar distanceFromPlane = p1ToCentre.dot(normal);
98
 
 
99
 
        if (distanceFromPlane < btScalar(0.))
100
 
        {
101
 
                //triangle facing the other way
102
 
        
103
 
                distanceFromPlane *= btScalar(-1.);
104
 
                normal *= btScalar(-1.);
105
 
        }
106
 
 
107
 
        ///todo: move this gContactBreakingThreshold into a proper structure
108
 
        extern btScalar gContactBreakingThreshold;
109
 
 
110
 
        btScalar contactMargin = gContactBreakingThreshold;
111
 
        bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
112
 
        bool isInsideShellPlane = distanceFromPlane < r;
113
 
        
114
 
        btScalar deltaDotNormal = delta.dot(normal);
115
 
        if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
116
 
                return false;
117
 
 
118
 
        // Check for contact / intersection
119
 
        bool hasContact = false;
120
 
        btVector3 contactPoint;
121
 
        if (isInsideContactPlane) {
122
 
                if (facecontains(c,vertices,normal)) {
123
 
                        // Inside the contact wedge - touches a point on the shell plane
124
 
                        hasContact = true;
125
 
                        contactPoint = c - normal*distanceFromPlane;
126
 
                } else {
127
 
                        // Could be inside one of the contact capsules
128
 
                        btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
129
 
                        btVector3 nearestOnEdge;
130
 
                        for (int i = 0; i < m_triangle->getNumEdges(); i++) {
131
 
                                
132
 
                                btPoint3 pa;
133
 
                                btPoint3 pb;
134
 
                                
135
 
                                m_triangle->getEdge(i,pa,pb);
136
 
 
137
 
                                btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
138
 
                                if (distanceSqr < contactCapsuleRadiusSqr) {
139
 
                                        // Yep, we're inside a capsule
140
 
                                        hasContact = true;
141
 
                                        contactPoint = nearestOnEdge;
142
 
                                }
143
 
                                
144
 
                        }
145
 
                }
146
 
        }
147
 
 
148
 
        if (hasContact) {
149
 
                btVector3 contactToCentre = c - contactPoint;
150
 
                btScalar distanceSqr = contactToCentre.length2();
151
 
                if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
152
 
                        btScalar distance = btSqrt(distanceSqr);
153
 
                        resultNormal = contactToCentre;
154
 
                        resultNormal.normalize();
155
 
                        point = contactPoint;
156
 
                        depth = -(r-distance);
157
 
                        return true;
158
 
                }
159
 
 
160
 
                if (delta.dot(contactToCentre) >= btScalar(0.0)) 
161
 
                        return false;
162
 
                
163
 
                // Moving towards the contact point -> collision
164
 
                point = contactPoint;
165
 
                timeOfImpact = btScalar(0.0);
166
 
                return true;
167
 
        }
168
 
        
169
 
        return false;
170
 
}
171
 
 
172
 
 
173
 
bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
174
 
{
175
 
        const btVector3* p1 = &vertices[0];
176
 
        const btVector3* p2 = &vertices[1];
177
 
        const btVector3* p3 = &vertices[2];
178
 
 
179
 
        btVector3 edge1( *p2 - *p1 );
180
 
        btVector3 edge2( *p3 - *p2 );
181
 
        btVector3 edge3( *p1 - *p3 );
182
 
 
183
 
        btVector3 p1_to_p( *p - *p1 );
184
 
        btVector3 p2_to_p( *p - *p2 );
185
 
        btVector3 p3_to_p( *p - *p3 );
186
 
 
187
 
        btVector3 edge1_normal( edge1.cross(normal));
188
 
        btVector3 edge2_normal( edge2.cross(normal));
189
 
        btVector3 edge3_normal( edge3.cross(normal));
190
 
        
191
 
        btScalar r1, r2, r3;
192
 
        r1 = edge1_normal.dot( p1_to_p );
193
 
        r2 = edge2_normal.dot( p2_to_p );
194
 
        r3 = edge3_normal.dot( p3_to_p );
195
 
        if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
196
 
             ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
197
 
                return true;
198
 
        return false;
199
 
 
200
 
}
 
1
/*
 
2
Bullet Continuous Collision Detection and Physics Library
 
3
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
4
 
 
5
This software is provided 'as-is', without any express or implied warranty.
 
6
In no event will the authors be held liable for any damages arising from the use of this software.
 
7
Permission is granted to anyone to use this software for any purpose, 
 
8
including commercial applications, and to alter it and redistribute it freely, 
 
9
subject to the following restrictions:
 
10
 
 
11
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
 
12
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
 
13
3. This notice may not be removed or altered from any source distribution.
 
14
*/
 
15
 
 
16
#include "LinearMath/btScalar.h"
 
17
#include "SphereTriangleDetector.h"
 
18
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
 
19
#include "BulletCollision/CollisionShapes/btSphereShape.h"
 
20
 
 
21
 
 
22
SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle)
 
23
:m_sphere(sphere),
 
24
m_triangle(triangle)
 
25
{
 
26
 
 
27
}
 
28
 
 
29
void    SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
 
30
{
 
31
 
 
32
        (void)debugDraw;
 
33
        const btTransform& transformA = input.m_transformA;
 
34
        const btTransform& transformB = input.m_transformB;
 
35
 
 
36
        btVector3 point,normal;
 
37
        btScalar timeOfImpact = btScalar(1.);
 
38
        btScalar depth = btScalar(0.);
 
39
//      output.m_distance = btScalar(1e30);
 
40
        //move sphere into triangle space
 
41
        btTransform     sphereInTr = transformB.inverseTimes(transformA);
 
42
 
 
43
        if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact))
 
44
        {
 
45
                output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
 
46
        }
 
47
 
 
48
}
 
49
 
 
50
#define MAX_OVERLAP btScalar(0.)
 
51
 
 
52
 
 
53
 
 
54
// See also geometrictools.com
 
55
// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
 
56
btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
 
57
        btVector3 diff = p - from;
 
58
        btVector3 v = to - from;
 
59
        btScalar t = v.dot(diff);
 
60
        
 
61
        if (t > 0) {
 
62
                btScalar dotVV = v.dot(v);
 
63
                if (t < dotVV) {
 
64
                        t /= dotVV;
 
65
                        diff -= t*v;
 
66
                } else {
 
67
                        t = 1;
 
68
                        diff -= v;
 
69
                }
 
70
        } else
 
71
                t = 0;
 
72
 
 
73
        nearest = from + t*v;
 
74
        return diff.dot(diff);  
 
75
}
 
76
 
 
77
bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
 
78
        btVector3 lp(p);
 
79
        btVector3 lnormal(normal);
 
80
        
 
81
        return pointInTriangle(vertices, lnormal, &lp);
 
82
}
 
83
 
 
84
///combined discrete/continuous sphere-triangle
 
85
bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact)
 
86
{
 
87
 
 
88
        const btVector3* vertices = &m_triangle->getVertexPtr(0);
 
89
        const btVector3& c = sphereCenter;
 
90
        btScalar r = m_sphere->getRadius();
 
91
 
 
92
        btVector3 delta (0,0,0);
 
93
 
 
94
        btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
 
95
        normal.normalize();
 
96
        btVector3 p1ToCentre = c - vertices[0];
 
97
        btScalar distanceFromPlane = p1ToCentre.dot(normal);
 
98
 
 
99
        if (distanceFromPlane < btScalar(0.))
 
100
        {
 
101
                //triangle facing the other way
 
102
        
 
103
                distanceFromPlane *= btScalar(-1.);
 
104
                normal *= btScalar(-1.);
 
105
        }
 
106
 
 
107
        ///todo: move this gContactBreakingThreshold into a proper structure
 
108
        extern btScalar gContactBreakingThreshold;
 
109
 
 
110
        btScalar contactMargin = gContactBreakingThreshold;
 
111
        bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
 
112
        bool isInsideShellPlane = distanceFromPlane < r;
 
113
        
 
114
        btScalar deltaDotNormal = delta.dot(normal);
 
115
        if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
 
116
                return false;
 
117
 
 
118
        // Check for contact / intersection
 
119
        bool hasContact = false;
 
120
        btVector3 contactPoint;
 
121
        if (isInsideContactPlane) {
 
122
                if (facecontains(c,vertices,normal)) {
 
123
                        // Inside the contact wedge - touches a point on the shell plane
 
124
                        hasContact = true;
 
125
                        contactPoint = c - normal*distanceFromPlane;
 
126
                } else {
 
127
                        // Could be inside one of the contact capsules
 
128
                        btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
 
129
                        btVector3 nearestOnEdge;
 
130
                        for (int i = 0; i < m_triangle->getNumEdges(); i++) {
 
131
                                
 
132
                                btPoint3 pa;
 
133
                                btPoint3 pb;
 
134
                                
 
135
                                m_triangle->getEdge(i,pa,pb);
 
136
 
 
137
                                btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
 
138
                                if (distanceSqr < contactCapsuleRadiusSqr) {
 
139
                                        // Yep, we're inside a capsule
 
140
                                        hasContact = true;
 
141
                                        contactPoint = nearestOnEdge;
 
142
                                }
 
143
                                
 
144
                        }
 
145
                }
 
146
        }
 
147
 
 
148
        if (hasContact) {
 
149
                btVector3 contactToCentre = c - contactPoint;
 
150
                btScalar distanceSqr = contactToCentre.length2();
 
151
                if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
 
152
                        btScalar distance = btSqrt(distanceSqr);
 
153
                        resultNormal = contactToCentre;
 
154
                        resultNormal.normalize();
 
155
                        point = contactPoint;
 
156
                        depth = -(r-distance);
 
157
                        return true;
 
158
                }
 
159
 
 
160
                if (delta.dot(contactToCentre) >= btScalar(0.0)) 
 
161
                        return false;
 
162
                
 
163
                // Moving towards the contact point -> collision
 
164
                point = contactPoint;
 
165
                timeOfImpact = btScalar(0.0);
 
166
                return true;
 
167
        }
 
168
        
 
169
        return false;
 
170
}
 
171
 
 
172
 
 
173
bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
 
174
{
 
175
        const btVector3* p1 = &vertices[0];
 
176
        const btVector3* p2 = &vertices[1];
 
177
        const btVector3* p3 = &vertices[2];
 
178
 
 
179
        btVector3 edge1( *p2 - *p1 );
 
180
        btVector3 edge2( *p3 - *p2 );
 
181
        btVector3 edge3( *p1 - *p3 );
 
182
 
 
183
        btVector3 p1_to_p( *p - *p1 );
 
184
        btVector3 p2_to_p( *p - *p2 );
 
185
        btVector3 p3_to_p( *p - *p3 );
 
186
 
 
187
        btVector3 edge1_normal( edge1.cross(normal));
 
188
        btVector3 edge2_normal( edge2.cross(normal));
 
189
        btVector3 edge3_normal( edge3.cross(normal));
 
190
        
 
191
        btScalar r1, r2, r3;
 
192
        r1 = edge1_normal.dot( p1_to_p );
 
193
        r2 = edge2_normal.dot( p2_to_p );
 
194
        r3 = edge3_normal.dot( p3_to_p );
 
195
        if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
 
196
             ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
 
197
                return true;
 
198
        return false;
 
199
 
 
200
}