Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @ 9353

Last change on this file since 9353 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

  • Property svn:eol-style set to native
File size: 6.0 KB
RevLine 
[1963]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
[2430]22SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
[1963]23:m_sphere(sphere),
[2430]24m_triangle(triangle),
25m_contactBreakingThreshold(contactBreakingThreshold)
[1963]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.);
[8351]40//      output.m_distance = btScalar(BT_LARGE_FLOAT);
[1963]41        //move sphere into triangle space
42        btTransform     sphereInTr = transformB.inverseTimes(transformA);
43
[2430]44        if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
[1963]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
61
62// See also geometrictools.com
63// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
64btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
65
66btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
67        btVector3 diff = p - from;
68        btVector3 v = to - from;
69        btScalar t = v.dot(diff);
70       
71        if (t > 0) {
72                btScalar dotVV = v.dot(v);
73                if (t < dotVV) {
74                        t /= dotVV;
75                        diff -= t*v;
76                } else {
77                        t = 1;
78                        diff -= v;
79                }
80        } else
81                t = 0;
82
83        nearest = from + t*v;
84        return diff.dot(diff); 
85}
86
87bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
88        btVector3 lp(p);
89        btVector3 lnormal(normal);
90       
91        return pointInTriangle(vertices, lnormal, &lp);
92}
93
[2430]94bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
[1963]95{
96
97        const btVector3* vertices = &m_triangle->getVertexPtr(0);
[8393]98       
99        btScalar radius = m_sphere->getRadius();
100        btScalar radiusWithThreshold = radius + contactBreakingThreshold;
[1963]101
102        btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
103        normal.normalize();
[8393]104        btVector3 p1ToCentre = sphereCenter - vertices[0];
[1963]105        btScalar distanceFromPlane = p1ToCentre.dot(normal);
106
107        if (distanceFromPlane < btScalar(0.))
108        {
109                //triangle facing the other way
110                distanceFromPlane *= btScalar(-1.);
111                normal *= btScalar(-1.);
112        }
113
[8393]114        bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
[1963]115       
116        // Check for contact / intersection
117        bool hasContact = false;
118        btVector3 contactPoint;
119        if (isInsideContactPlane) {
[8393]120                if (facecontains(sphereCenter,vertices,normal)) {
[1963]121                        // Inside the contact wedge - touches a point on the shell plane
122                        hasContact = true;
[8393]123                        contactPoint = sphereCenter - normal*distanceFromPlane;
[1963]124                } else {
125                        // Could be inside one of the contact capsules
[8393]126                        btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
[1963]127                        btVector3 nearestOnEdge;
128                        for (int i = 0; i < m_triangle->getNumEdges(); i++) {
129                               
[2430]130                                btVector3 pa;
131                                btVector3 pb;
[1963]132                               
133                                m_triangle->getEdge(i,pa,pb);
134
[8393]135                                btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge);
[1963]136                                if (distanceSqr < contactCapsuleRadiusSqr) {
137                                        // Yep, we're inside a capsule
138                                        hasContact = true;
139                                        contactPoint = nearestOnEdge;
140                                }
141                               
142                        }
143                }
144        }
145
146        if (hasContact) {
[8393]147                btVector3 contactToCentre = sphereCenter - contactPoint;
[1963]148                btScalar distanceSqr = contactToCentre.length2();
[8393]149
150                if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
151                {
152                        if (distanceSqr>SIMD_EPSILON)
153                        {
154                                btScalar distance = btSqrt(distanceSqr);
155                                resultNormal = contactToCentre;
156                                resultNormal.normalize();
157                                point = contactPoint;
158                                depth = -(radius-distance);
159                        } else
160                        {
161                                btScalar distance = 0.f;
162                                resultNormal = normal;
163                                point = contactPoint;
164                                depth = -radius;
165                        }
[1963]166                        return true;
167                }
168        }
169       
170        return false;
171}
172
173
174bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
175{
176        const btVector3* p1 = &vertices[0];
177        const btVector3* p2 = &vertices[1];
178        const btVector3* p3 = &vertices[2];
179
180        btVector3 edge1( *p2 - *p1 );
181        btVector3 edge2( *p3 - *p2 );
182        btVector3 edge3( *p1 - *p3 );
183
184        btVector3 p1_to_p( *p - *p1 );
185        btVector3 p2_to_p( *p - *p2 );
186        btVector3 p3_to_p( *p - *p3 );
187
188        btVector3 edge1_normal( edge1.cross(normal));
189        btVector3 edge2_normal( edge2.cross(normal));
190        btVector3 edge3_normal( edge3.cross(normal));
191       
192        btScalar r1, r2, r3;
193        r1 = edge1_normal.dot( p1_to_p );
194        r2 = edge2_normal.dot( p2_to_p );
195        r3 = edge3_normal.dot( p3_to_p );
196        if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
197             ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
198                return true;
199        return false;
200
201}
Note: See TracBrowser for help on using the repository browser.