AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

removed SphereSphereCollisionAlgorithm.cpp (is now part of core Bullet/src) and added ODE BoxBoxCollisionAlgorithm/BoxBoxDetector for comparison

Browse Source
This commit is contained in:
ejcoumans 2006-10-27 21:01:16 +00:00
parent e2093823ff
commit b8376a1673
5 changed files with 796 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.cpp Normal file
View File

@ -0,0 +1,72 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "BoxBoxCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BoxBoxDetector.h"
BoxBoxCollisionAlgorithm::BoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* obj0,btCollisionObject* obj1)
: btCollisionAlgorithm(ci),
m_ownManifold(false),
m_manifoldPtr(mf)
{
if (!m_manifoldPtr && m_dispatcher->needsCollision(obj0,obj1))
{
m_manifoldPtr = m_dispatcher->getNewManifold(obj0,obj1);
m_ownManifold = true;
}
}
BoxBoxCollisionAlgorithm::~BoxBoxCollisionAlgorithm()
{
if (m_ownManifold)
{
if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr);
}
}
void BoxBoxCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
return;
btCollisionObject* col0 = body0;
btCollisionObject* col1 = body1;
btBoxShape* box0 = (btBoxShape*)col0->m_collisionShape;
btBoxShape* box1 = (btBoxShape*)col1->m_collisionShape;
/// report a contact. internally this will be kept persistent, and contact reduction is done
resultOut->setPersistentManifold(m_manifoldPtr);
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_maximumDistanceSquared;
input.m_transformA = body0->m_worldTransform;
input.m_transformB = body1->m_worldTransform;
BoxBoxDetector detector(box0,box1);
detector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
}
float BoxBoxCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
//not yet
return 1.f;
}

26
Extras/AlternativeCollisionAlgorithms/SphereSphereCollisionAlgorithm.h → Extras/AlternativeCollisionAlgorithms/BoxBoxCollisionAlgorithm.h
View File

@ -13,42 +13,40 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H
#define SPHERE_SPHERE_COLLISION_ALGORITHM_H
#ifndef BOX_BOX__COLLISION_ALGORITHM_H
#define BOX_BOX__COLLISION_ALGORITHM_H
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class btPersistentManifold;
/// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
/// Other features are frame-coherency (persistent data) and collision response.
/// Also provides the most basic sample for custom/user btCollisionAlgorithm
class btSphereSphereCollisionAlgorithm : public btCollisionAlgorithm
///box-box collision detection
class BoxBoxCollisionAlgorithm : public btCollisionAlgorithm
{
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
public:
btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
BoxBoxCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btCollisionAlgorithm(ci) {}
virtual void processCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual float calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
virtual float calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
BoxBoxCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
virtual ~btSphereSphereCollisionAlgorithm();
virtual ~BoxBoxCollisionAlgorithm();
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
return new btSphereSphereCollisionAlgorithm(0,ci,proxy0,proxy1);
return new BoxBoxCollisionAlgorithm(0,ci,body0,body1);
}
};
};
#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H
#endif //BOX_BOX__COLLISION_ALGORITHM_H

675
Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.cpp Normal file
View File

@ -0,0 +1,675 @@
/*************************************************************************
* *
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser bteral Public License as published by the Free *
* Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. The text of the GNU Lesser *
* bteral Public License is included with this library in the *
* file LICENSE.TXT. *
* (2) The BSD-style license that is included with this library in *
* the file LICENSE-BSD.TXT. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include "BoxBoxDetector.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"
#include <float.h>
BoxBoxDetector::BoxBoxDetector(btBoxShape* box1,btBoxShape* box2)
: m_box1(box1),
m_box2(box2)
{
}
// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
// generate contact points. this returns 0 if there is no contact otherwise
// it returns the number of contacts generated.
// `normal' returns the contact normal.
// `depth' returns the maximum penetration depth along that normal.
// `return_code' returns a number indicating the type of contact that was
// detected:
// 1,2,3 = box 2 intersects with a face of box 1
// 4,5,6 = box 1 intersects with a face of box 2
// 7..15 = edge-edge contact
// `maxc' is the maximum number of contacts allowed to be generated, i.e.
// the size of the `contact' array.
// `contact' and `skip' are the contact array information provided to the
// collision functions. this function only fills in the position and depth
// fields.
struct dContactGeom;
#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
#define dInfinity FLT_MAX
/*PURE_INLINE btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
PURE_INLINE btScalar dDOT13 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,3); }
PURE_INLINE btScalar dDOT31 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,1); }
PURE_INLINE btScalar dDOT33 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,3,3); }
*/
static btScalar dDOT (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
static btScalar dDOT44 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,4); }
static btScalar dDOT41 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,4,1); }
static btScalar dDOT14 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,4); }
#define dMULTIPLYOP1_331(A,op,B,C) \
do { \
(A)[0] op dDOT41((B),(C)); \
(A)[1] op dDOT41((B+1),(C)); \
(A)[2] op dDOT41((B+2),(C)); \
} while(0)
#define dMULTIPLYOP0_331(A,op,B,C) \
do { \
(A)[0] op dDOT((B),(C)); \
(A)[1] op dDOT((B+4),(C)); \
(A)[2] op dDOT((B+8),(C)); \
} while(0)
#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
typedef btScalar dMatrix3[4*3];
void dLineClosestApproach (const btVector3 pa, const btVector3 ua,
const btVector3 pb, const btVector3 ub,
btScalar *alpha, btScalar *beta)
{
btVector3 p;
p[0] = pb[0] - pa[0];
p[1] = pb[1] - pa[1];
p[2] = pb[2] - pa[2];
btScalar uaub = dDOT(ua,ub);
btScalar q1 = dDOT(ua,p);
btScalar q2 = -dDOT(ub,p);
btScalar d = 1-uaub*uaub;
if (d <= btScalar(0.0001f)) {
// @@@ this needs to be made more robust
*alpha = 0;
*beta = 0;
}
else {
d = 1.f/d;
*alpha = (q1 + uaub*q2)*d;
*beta = (uaub*q1 + q2)*d;
}
}
// find all the intersection points between the 2D rectangle with vertices
// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
//
// the intersection points are returned as x,y pairs in the 'ret' array.
// the number of intersection points is returned by the function (this will
// be in the range 0 to 8).
static int intersectRectQuad2 (btScalar h[2], btScalar p[8], btScalar ret[16])
{
// q (and r) contain nq (and nr) coordinate points for the current (and
// chopped) polygons
int nq=4,nr;
btScalar buffer[16];
btScalar *q = p;
btScalar *r = ret;
for (int dir=0; dir <= 1; dir++) {
// direction notation: xy[0] = x axis, xy[1] = y axis
for (int sign=-1; sign <= 1; sign += 2) {
// chop q along the line xy[dir] = sign*h[dir]
btScalar *pq = q;
btScalar *pr = r;
nr = 0;
for (int i=nq; i > 0; i--) {
// go through all points in q and all lines between adjacent points
if (sign*pq[dir] < h[dir]) {
// this point is inside the chopping line
pr[0] = pq[0];
pr[1] = pq[1];
pr += 2;
nr++;
if (nr & 8) {
q = r;
goto done;
}
}
btScalar *nextq = (i > 1) ? pq+2 : q;
if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
// this line crosses the chopping line
pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
(nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
pr[dir] = sign*h[dir];
pr += 2;
nr++;
if (nr & 8) {
q = r;
goto done;
}
}
pq += 2;
}
q = r;
r = (q==ret) ? buffer : ret;
nq = nr;
}
}
done:
if (q != ret) memcpy (ret,q,nr*2*sizeof(btScalar));
return nr;
}
#define dAtan2(y,x) ((float)atan2f((y),(x))) /* arc tangent with 2 args */
#define M__PI 3.14159265f
// given n points in the plane (array p, of size 2*n), generate m points that
// best represent the whole set. the definition of 'best' here is not
// predetermined - the idea is to select points that give good box-box
// collision detection behavior. the chosen point indexes are returned in the
// array iret (of size m). 'i0' is always the first entry in the array.
// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
// in the range [0..n-1].
void cullPoints2 (int n, btScalar p[], int m, int i0, int iret[])
{
// compute the centroid of the polygon in cx,cy
int i,j;
btScalar a,cx,cy,q;
if (n==1) {
cx = p[0];
cy = p[1];
}
else if (n==2) {
cx = btScalar(0.5)*(p[0] + p[2]);
cy = btScalar(0.5)*(p[1] + p[3]);
}
else {
a = 0;
cx = 0;
cy = 0;
for (i=0; i<(n-1); i++) {
q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
a += q;
cx += q*(p[i*2]+p[i*2+2]);
cy += q*(p[i*2+1]+p[i*2+3]);
}
q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
a = 1.f/(btScalar(3.0)*(a+q));
cx = a*(cx + q*(p[n*2-2]+p[0]));
cy = a*(cy + q*(p[n*2-1]+p[1]));
}
// compute the angle of each point w.r.t. the centroid
btScalar A[8];
for (i=0; i<n; i++) A[i] = dAtan2(p[i*2+1]-cy,p[i*2]-cx);
// search for points that have angles closest to A[i0] + i*(2*pi/m).
int avail[8];
for (i=0; i<n; i++) avail[i] = 1;
avail[i0] = 0;
iret[0] = i0;
iret++;
for (j=1; j<m; j++) {
a = btScalar(j)*(2*M__PI/m) + A[i0];
if (a > M__PI) a -= 2*M__PI;
btScalar maxdiff=1e9,diff;
#ifndef dNODEBUG
*iret = i0; // iret is not allowed to keep this value
#endif
for (i=0; i<n; i++) {
if (avail[i]) {
diff = fabsf (A[i]-a);
if (diff > M__PI) diff = 2*M__PI - diff;
if (diff < maxdiff) {
maxdiff = diff;
*iret = i;
}
}
}
#ifndef dNODEBUG
ASSERT (*iret != i0); // ensure iret got set
#endif
avail[*iret] = 0;
iret++;
}
}
int dBoxBox2 (const btVector3 p1, const dMatrix3 R1,
const btVector3 side1, const btVector3 p2,
const dMatrix3 R2, const btVector3 side2,
btVector3& normal, btScalar *depth, int *return_code,
int maxc, dContactGeom *contact, int skip,btDiscreteCollisionDetectorInterface::Result& output)
{
const btScalar fudge_factor = btScalar(1.05);
btVector3 p,pp,normalC;
const btScalar *normalR = 0;
btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
int i,j,invert_normal,code;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2[0] - p1[0];
p[1] = p2[1] - p1[1];
p[2] = p2[2] - p1[2];
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
// get side lengths / 2
A[0] = side1[0]*btScalar(0.5);
A[1] = side1[1]*btScalar(0.5);
A[2] = side1[2]*btScalar(0.5);
B[0] = side2[0]*btScalar(0.5);
B[1] = side2[1]*btScalar(0.5);
B[2] = side2[2]*btScalar(0.5);
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
Q11 = fabsf(R11); Q12 = fabsf(R12); Q13 = fabsf(R13);
Q21 = fabsf(R21); Q22 = fabsf(R22); Q23 = fabsf(R23);
Q31 = fabsf(R31); Q32 = fabsf(R32); Q33 = fabsf(R33);
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
#define TST(expr1,expr2,norm,cc) \
s2 = fabsf(expr1) - (expr2); \
if (s2 > 0) return 0; \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1) < 0); \
code = (cc); \
}
s = -dInfinity;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
// separating axis = v1,v2,v3
TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
s2 = fabsf(expr1) - (expr2); \
if (s2 > 0) return 0; \
l = sqrtf((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
if (l > 0) { \
s2 /= l; \
if (s2*fudge_factor > s) { \
s = s2; \
normalR = 0; \
normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
invert_normal = ((expr1) < 0); \
code = (cc); \
} \
}
// separating axis = u1 x (v1,v2,v3)
TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
if (!code) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if (normalR) {
normal[0] = normalR[0];
normal[1] = normalR[4];
normal[2] = normalR[8];
}
else {
dMULTIPLY0_331 (normal,R1,normalC);
}
if (invert_normal) {
normal[0] = -normal[0];
normal[1] = -normal[1];
normal[2] = -normal[2];
}
*depth = -s;
// compute contact point(s)
if (code > 6) {
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
btVector3 pa;
btScalar sign;
for (i=0; i<3; i++) pa[i] = p1[i];
for (j=0; j<3; j++) {
sign = (dDOT14(normal,R1+j) > 0) ? btScalar(1.0) : btScalar(-1.0);
for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
}
// find a point pb on the intersecting edge of box 2
btVector3 pb;
for (i=0; i<3; i++) pb[i] = p2[i];
for (j=0; j<3; j++) {
sign = (dDOT14(normal,R2+j) > 0) ? btScalar(-1.0) : btScalar(1.0);
for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
}
btScalar alpha,beta;
btVector3 ua,ub;
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
for (i=0; i<3; i++) pb[i] += ub[i]*beta;
{
//contact[0].pos[i] = btScalar(0.5)*(pa[i]+pb[i]);
//contact[0].depth = *depth;
btVector3 pointInWorld;
for (i=0; i<3; i++)
pointInWorld[i] = (pa[i]+pb[i])*btScalar(0.5);
output.addContactPoint(-normal,pointInWorld,-*depth);
*return_code = code;
}
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) {
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else {
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
btVector3 normal2,nr,anr;
if (code <= 3) {
normal2[0] = normal[0];
normal2[1] = normal[1];
normal2[2] = normal[2];
}
else {
normal2[0] = -normal[0];
normal2[1] = -normal[1];
normal2[2] = -normal[2];
}
dMULTIPLY1_331 (nr,Rb,normal2);
anr[0] = fabsf (nr[0]);
anr[1] = fabsf (nr[1]);
anr[2] = fabsf (nr[2]);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr,a1,a2;
if (anr[1] > anr[0]) {
if (anr[1] > anr[2]) {
a1 = 0;
lanr = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else {
if (anr[0] > anr[2]) {
lanr = 0;
a1 = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
btVector3 center;
if (nr[lanr] < 0) {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
}
// find the normal and non-normal axis numbers of the reference box
int codeN,code1,code2;
if (code <= 3) codeN = code-1; else codeN = code-4;
if (codeN==0) {
code1 = 1;
code2 = 2;
}
else if (codeN==1) {
code1 = 0;
code2 = 2;
}
else {
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
btScalar quad[8]; // 2D coordinate of incident face (x,y pairs)
btScalar c1,c2,m11,m12,m21,m22;
c1 = dDOT14 (center,Ra+code1);
c2 = dDOT14 (center,Ra+code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = dDOT44 (Ra+code1,Rb+a1);
m12 = dDOT44 (Ra+code1,Rb+a2);
m21 = dDOT44 (Ra+code2,Rb+a1);
m22 = dDOT44 (Ra+code2,Rb+a2);
{
btScalar k1 = m11*Sb[a1];
btScalar k2 = m21*Sb[a1];
btScalar k3 = m12*Sb[a2];
btScalar k4 = m22*Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
btScalar rect[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
btScalar ret[16];
int n = intersectRectQuad2 (rect,quad,ret);
if (n < 1) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
btScalar point[3*8]; // penetrating contact points
btScalar dep[8]; // depths for those points
btScalar det1 = 1.f/(m11*m22 - m12*m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
btScalar k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
for (i=0; i<3; i++) point[cnum*3+i] =
center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
if (dep[cnum] >= 0) {
ret[cnum*2] = ret[j*2];
ret[cnum*2+1] = ret[j*2+1];
cnum++;
}
}
if (cnum < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
if (maxc > cnum) maxc = cnum;
if (maxc < 1) maxc = 1;
if (cnum <= maxc) {
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++) {
//AddContactPoint...
//dContactGeom *con = CONTACT(contact,skip*j);
//for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
//con->depth = dep[j];
btVector3 pointInWorld;
for (i=0; i<3; i++)
pointInWorld[i] = point[j*3+i] + pa[i];
output.addContactPoint(-normal,pointInWorld,-dep[j]);
}
}
else {
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
btScalar maxdepth = dep[0];
for (i=1; i<cnum; i++) {
if (dep[i] > maxdepth) {
maxdepth = dep[i];
i1 = i;
}
}
int iret[8];
cullPoints2 (cnum,ret,maxc,i1,iret);
for (j=0; j < maxc; j++) {
// dContactGeom *con = CONTACT(contact,skip*j);
// for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
// con->depth = dep[iret[j]];
btVector3 posInWorld;
for (i=0; i<3; i++)
posInWorld[i] = point[iret[j]*3+i] + pa[i];
output.addContactPoint(-normal,posInWorld,-dep[iret[j]]);
}
cnum = maxc;
}
*return_code = code;
return cnum;
}
void BoxBoxDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
{
const btTransform& transformA = input.m_transformA;
const btTransform& transformB = input.m_transformB;
int skip = 0;
dContactGeom *contact = 0;
dMatrix3 R1;
dMatrix3 R2;
for (int i=0;i<3;i++)
{
for (int j=0;j<3;j++)
{
R1[i+4*j] = transformA.getBasis()[j][i];
R2[i+4*j] = transformB.getBasis()[j][i];
}
}
btVector3 normal;
btScalar depth;
int return_code;
int maxc = 10;
dBoxBox2 (transformA.getOrigin(),
R1,
2.f*m_box1->getHalfExtents(),
transformB.getOrigin(),
R2,
2.f*m_box2->getHalfExtents(),
normal, &depth, &return_code,
maxc, contact, skip,
output
);
}

37
Extras/AlternativeCollisionAlgorithms/BoxBoxDetector.h Normal file
View File

@ -0,0 +1,37 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BOX_BOX_DETECTOR_H
#define BOX_BOX_DETECTOR_H
class btBoxShape;
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
struct BoxBoxDetector : public btDiscreteCollisionDetectorInterface
{
btBoxShape* m_box1;
btBoxShape* m_box2;
public:
BoxBoxDetector(btBoxShape* box1,btBoxShape* box2);
virtual ~BoxBoxDetector() {};
virtual void getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw);
};
#endif //BOX_BOX_DETECTOR_H

81
Extras/AlternativeCollisionAlgorithms/SphereSphereCollisionAlgorithm.cpp
View File

@ -1,81 +0,0 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btSphereSphereCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
: btCollisionAlgorithm(ci),
m_ownManifold(false),
m_manifoldPtr(mf)
{
if (!m_manifoldPtr && m_dispatcher->needsCollision(*proxy0,*proxy1))
{
m_manifoldPtr = m_dispatcher->getNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
m_ownManifold = true;
}
}
btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
{
if (m_ownManifold)
{
if (m_manifoldPtr)
m_dispatcher->releaseManifold(m_manifoldPtr);
}
}
void btSphereSphereCollisionAlgorithm::processCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
{
if (!m_manifoldPtr)
return;
btCollisionObject* col0 = static_cast<btCollisionObject*>(proxy0->m_clientObject);
btCollisionObject* col1 = static_cast<btCollisionObject*>(proxy1->m_clientObject);
btSphereShape* sphere0 = (btSphereShape*)col0->m_collisionShape;
btSphereShape* sphere1 = (btSphereShape*)col1->m_collisionShape;
btVector3 diff = col0->m_worldTransform.getOrigin()- col1->m_worldTransform.getOrigin();
float len = diff.length();
btScalar radius0 = sphere0->getRadius();
btScalar radius1 = sphere1->getRadius();
///iff distance positive, don't generate a new contact
if ( len > (radius0+radius1))
return;
///distance (negative means penetration)
btScalar dist = len - (radius0+radius1);
btVector3 normalOnSurfaceB = diff / len;
///point on A (worldspace)
btVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
btVector3 pos1 = col1->m_worldTransform.getOrigin() + radius1* normalOnSurfaceB;
/// report a contact. internally this will be kept persistent, and contact reduction is done
btManifoldResult* resultOut = m_dispatcher->getNewManifoldResult(col0,col1,m_manifoldPtr);
resultOut->addContactPoint(normalOnSurfaceB,pos1,dist);
m_dispatcher->releaseManifoldResult(resultOut);
}
float btSphereSphereCollisionAlgorithm::calculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
{
//not yet
return 1.f;
}