Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "LinearMath/btScalar.h"
00017 #include "SphereTriangleDetector.h"
00018 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
00019 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00020
00021
00022 SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
00023 :m_sphere(sphere),
00024 m_triangle(triangle),
00025 m_contactBreakingThreshold(contactBreakingThreshold)
00026 {
00027
00028 }
00029
00030 void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
00031 {
00032
00033 (void)debugDraw;
00034 const btTransform& transformA = input.m_transformA;
00035 const btTransform& transformB = input.m_transformB;
00036
00037 btVector3 point,normal;
00038 btScalar timeOfImpact = btScalar(1.);
00039 btScalar depth = btScalar(0.);
00040
00041
00042 btTransform sphereInTr = transformB.inverseTimes(transformA);
00043
00044 if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
00045 {
00046 if (swapResults)
00047 {
00048 btVector3 normalOnB = transformB.getBasis()*normal;
00049 btVector3 normalOnA = -normalOnB;
00050 btVector3 pointOnA = transformB*point+normalOnB*depth;
00051 output.addContactPoint(normalOnA,pointOnA,depth);
00052 } else
00053 {
00054 output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
00055 }
00056 }
00057
00058 }
00059
00060
00061
00062
00063
00064 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
00065
00066 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
00067 btVector3 diff = p - from;
00068 btVector3 v = to - from;
00069 btScalar t = v.dot(diff);
00070
00071 if (t > 0) {
00072 btScalar dotVV = v.dot(v);
00073 if (t < dotVV) {
00074 t /= dotVV;
00075 diff -= t*v;
00076 } else {
00077 t = 1;
00078 diff -= v;
00079 }
00080 } else
00081 t = 0;
00082
00083 nearest = from + t*v;
00084 return diff.dot(diff);
00085 }
00086
00087 bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) {
00088 btVector3 lp(p);
00089 btVector3 lnormal(normal);
00090
00091 return pointInTriangle(vertices, lnormal, &lp);
00092 }
00093
00094 bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
00095 {
00096
00097 const btVector3* vertices = &m_triangle->getVertexPtr(0);
00098
00099 btScalar radius = m_sphere->getRadius();
00100 btScalar radiusWithThreshold = radius + contactBreakingThreshold;
00101
00102 btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
00103 normal.normalize();
00104 btVector3 p1ToCentre = sphereCenter - vertices[0];
00105 btScalar distanceFromPlane = p1ToCentre.dot(normal);
00106
00107 if (distanceFromPlane < btScalar(0.))
00108 {
00109
00110 distanceFromPlane *= btScalar(-1.);
00111 normal *= btScalar(-1.);
00112 }
00113
00114 bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
00115
00116
00117 bool hasContact = false;
00118 btVector3 contactPoint;
00119 if (isInsideContactPlane) {
00120 if (facecontains(sphereCenter,vertices,normal)) {
00121
00122 hasContact = true;
00123 contactPoint = sphereCenter - normal*distanceFromPlane;
00124 } else {
00125
00126 btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
00127 btVector3 nearestOnEdge;
00128 for (int i = 0; i < m_triangle->getNumEdges(); i++) {
00129
00130 btVector3 pa;
00131 btVector3 pb;
00132
00133 m_triangle->getEdge(i,pa,pb);
00134
00135 btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge);
00136 if (distanceSqr < contactCapsuleRadiusSqr) {
00137
00138 hasContact = true;
00139 contactPoint = nearestOnEdge;
00140 }
00141
00142 }
00143 }
00144 }
00145
00146 if (hasContact) {
00147 btVector3 contactToCentre = sphereCenter - contactPoint;
00148 btScalar distanceSqr = contactToCentre.length2();
00149
00150 if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
00151 {
00152 if (distanceSqr>SIMD_EPSILON)
00153 {
00154 btScalar distance = btSqrt(distanceSqr);
00155 resultNormal = contactToCentre;
00156 resultNormal.normalize();
00157 point = contactPoint;
00158 depth = -(radius-distance);
00159 } else
00160 {
00161 resultNormal = normal;
00162 point = contactPoint;
00163 depth = -radius;
00164 }
00165 return true;
00166 }
00167 }
00168
00169 return false;
00170 }
00171
00172
00173 bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
00174 {
00175 const btVector3* p1 = &vertices[0];
00176 const btVector3* p2 = &vertices[1];
00177 const btVector3* p3 = &vertices[2];
00178
00179 btVector3 edge1( *p2 - *p1 );
00180 btVector3 edge2( *p3 - *p2 );
00181 btVector3 edge3( *p1 - *p3 );
00182
00183 btVector3 p1_to_p( *p - *p1 );
00184 btVector3 p2_to_p( *p - *p2 );
00185 btVector3 p3_to_p( *p - *p3 );
00186
00187 btVector3 edge1_normal( edge1.cross(normal));
00188 btVector3 edge2_normal( edge2.cross(normal));
00189 btVector3 edge3_normal( edge3.cross(normal));
00190
00191 btScalar r1, r2, r3;
00192 r1 = edge1_normal.dot( p1_to_p );
00193 r2 = edge2_normal.dot( p2_to_p );
00194 r3 = edge3_normal.dot( p3_to_p );
00195 if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
00196 ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
00197 return true;
00198 return false;
00199
00200 }