Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/environment/src/external/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @ 8206

Last change on this file since 8206 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

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