Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

merged most of the changes from the branch into trunk, except for COLLADA, libxml and glut glitches.

Browse Source 
Still need to verify to make sure no unwanted renaming is introduced.
This commit is contained in:
ejcoumans
2006-09-27 20:43:51 +00:00
parent d1e9a885f3
commit eb23bb5c0c
263 changed files with 7528 additions and 6714 deletions
Download Patch File Download Diff File Expand all files Collapse all files

98
Extras/AlgebraicCCD/BU_AlgebraicPolynomialSolver.cpp
View File

@@ -16,23 +16,23 @@ subject to the following restrictions:
#include "BU_AlgebraicPolynomialSolver.h"
#include <math.h>
#include <SimdMinMax.h>
#include <btSimdMinMax.h>
int BU_AlgebraicPolynomialSolver::Solve2Quadratic(SimdScalar p, SimdScalar q)
int BU_AlgebraicPolynomialSolver::Solve2Quadratic(btScalar p, btScalar q)
{
SimdScalar basic_h_local;
SimdScalar basic_h_local_delta;
btScalar basic_h_local;
btScalar basic_h_local_delta;
basic_h_local = p * 0.5f;
basic_h_local_delta = basic_h_local * basic_h_local - q;
if (basic_h_local_delta > 0.0f) {
basic_h_local_delta = SimdSqrt(basic_h_local_delta);
basic_h_local_delta = btSqrt(basic_h_local_delta);
m_roots[0] = - basic_h_local + basic_h_local_delta;
m_roots[1] = - basic_h_local - basic_h_local_delta;
return 2;
}
else if (SimdGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) {
else if (btGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) {
m_roots[0] = - basic_h_local;
return 1;
}
@@ -42,13 +42,13 @@ int BU_AlgebraicPolynomialSolver::Solve2Quadratic(SimdScalar p, SimdScalar q)
}
int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c)
int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(btScalar a,btScalar b, btScalar c)
{
SimdScalar radical = b * b - 4.0f * a * c;
btScalar radical = b * b - 4.0f * a * c;
if(radical >= 0.f)
{
SimdScalar sqrtRadical = SimdSqrt(radical);
SimdScalar idenom = 1.0f/(2.0f * a);
btScalar sqrtRadical = btSqrt(radical);
btScalar idenom = 1.0f/(2.0f * a);
m_roots[0]=(-b + sqrtRadical) * idenom;
m_roots[1]=(-b - sqrtRadical) * idenom;
return 2;
@@ -58,8 +58,8 @@ int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b,
#define cubic_rt(x) \
((x) > 0.0f ? SimdPow((SimdScalar)(x), 0.333333333333333333333333f) : \
((x) < 0.0f ? -SimdPow((SimdScalar)-(x), 0.333333333333333333333333f) : 0.0f))
((x) > 0.0f ? btPow((btScalar)(x), 0.333333333333333333333333f) : \
((x) < 0.0f ? -btPow((btScalar)-(x), 0.333333333333333333333333f) : 0.0f))
@@ -72,26 +72,26 @@ int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b,
/* it returns the number of different roots found, and stores the roots in */
/* roots[0,2]. it returns -1 for a degenerate equation 0 = 0. */
/* */
int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c)
int BU_AlgebraicPolynomialSolver::Solve3Cubic(btScalar lead, btScalar a, btScalar b, btScalar c)
{
SimdScalar p, q, r;
SimdScalar delta, u, phi;
SimdScalar dummy;
btScalar p, q, r;
btScalar delta, u, phi;
btScalar dummy;
if (lead != 1.0) {
/* */
/* transform into normal form: x^3 + a x^2 + b x + c = 0 */
/* */
if (SimdEqual(lead, SIMD_EPSILON)) {
if (btEqual(lead, SIMD_EPSILON)) {
/* */
/* we have a x^2 + b x + c = 0 */
/* */
if (SimdEqual(a, SIMD_EPSILON)) {
if (btEqual(a, SIMD_EPSILON)) {
/* */
/* we have b x + c = 0 */
/* */
if (SimdEqual(b, SIMD_EPSILON)) {
if (SimdEqual(c, SIMD_EPSILON)) {
if (btEqual(b, SIMD_EPSILON)) {
if (btEqual(c, SIMD_EPSILON)) {
return -1;
}
else {
@@ -128,8 +128,8 @@ int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, Sim
/* */
/* now use Cardano's formula */
/* */
if (SimdEqual(p, SIMD_EPSILON)) {
if (SimdEqual(q, SIMD_EPSILON)) {
if (btEqual(p, SIMD_EPSILON)) {
if (btEqual(q, SIMD_EPSILON)) {
/* */
/* one triple root */
/* */
@@ -151,7 +151,7 @@ int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, Sim
/* */
/* one real and two complex roots. note that v = -p / u. */
/* */
u = -q + SimdSqrt(delta);
u = -q + btSqrt(delta);
u = cubic_rt(u);
m_roots[0] = u - p / u - a;
return 1;
@@ -160,19 +160,19 @@ int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, Sim
/* */
/* Casus irreducibilis: we have three real roots */
/* */
r = SimdSqrt(-p);
r = btSqrt(-p);
p *= -r;
r *= 2.0;
phi = SimdAcos(-q / p) / 3.0f;
phi = btAcos(-q / p) / 3.0f;
dummy = SIMD_2_PI / 3.0f;
m_roots[0] = r * SimdCos(phi) - a;
m_roots[1] = r * SimdCos(phi + dummy) - a;
m_roots[2] = r * SimdCos(phi - dummy) - a;
m_roots[0] = r * btCos(phi) - a;
m_roots[1] = r * btCos(phi + dummy) - a;
m_roots[2] = r * btCos(phi - dummy) - a;
return 3;
}
else {
/* */
/* one single and one SimdScalar root */
/* one single and one btScalar root */
/* */
r = cubic_rt(-q);
m_roots[0] = 2.0f * r - a;
@@ -191,31 +191,31 @@ int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, Sim
/* it returns the number of different roots found, and stores the roots in */
/* roots[0,3]. it returns -1 for a degenerate equation 0 = 0. */
/* */
int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d)
int BU_AlgebraicPolynomialSolver::Solve4Quartic(btScalar lead, btScalar a, btScalar b, btScalar c, btScalar d)
{
SimdScalar p, q ,r;
SimdScalar u, v, w;
btScalar p, q ,r;
btScalar u, v, w;
int i, num_roots, num_tmp;
//SimdScalar tmp[2];
//btScalar tmp[2];
if (lead != 1.0) {
/* */
/* transform into normal form: x^4 + a x^3 + b x^2 + c x + d = 0 */
/* */
if (SimdEqual(lead, SIMD_EPSILON)) {
if (btEqual(lead, SIMD_EPSILON)) {
/* */
/* we have a x^3 + b x^2 + c x + d = 0 */
/* */
if (SimdEqual(a, SIMD_EPSILON)) {
if (btEqual(a, SIMD_EPSILON)) {
/* */
/* we have b x^2 + c x + d = 0 */
/* */
if (SimdEqual(b, SIMD_EPSILON)) {
if (btEqual(b, SIMD_EPSILON)) {
/* */
/* we have c x + d = 0 */
/* */
if (SimdEqual(c, SIMD_EPSILON)) {
if (SimdEqual(d, SIMD_EPSILON)) {
if (btEqual(c, SIMD_EPSILON)) {
if (btEqual(d, SIMD_EPSILON)) {
return -1;
}
else {
@@ -254,7 +254,7 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
p = b - 6.0f * a * a;
q = a * (8.0f * a * a - 2.0f * b) + c;
r = a * (a * (b - 3.f * a * a) - c) + d;
if (SimdEqual(q, SIMD_EPSILON)) {
if (btEqual(q, SIMD_EPSILON)) {
/* */
/* biquadratic equation: y^4 + p y^2 + r = 0. */
/* */
@@ -263,23 +263,23 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
if (m_roots[0] > 0.0f) {
if (num_roots > 1) {
if ((m_roots[1] > 0.0f) && (m_roots[1] != m_roots[0])) {
u = SimdSqrt(m_roots[1]);
u = btSqrt(m_roots[1]);
m_roots[2] = u - a;
m_roots[3] = -u - a;
u = SimdSqrt(m_roots[0]);
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 4;
}
else {
u = SimdSqrt(m_roots[0]);
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 2;
}
}
else {
u = SimdSqrt(m_roots[0]);
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 2;
@@ -288,7 +288,7 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
}
return 0;
}
else if (SimdEqual(r, SIMD_EPSILON)) {
else if (btEqual(r, SIMD_EPSILON)) {
/* */
/* no absolute term: y (y^3 + p y + q) = 0. */
/* */
@@ -321,17 +321,17 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
u = w * w - r;
v = 2.0f * w - p;
if (SimdEqual(u, SIMD_EPSILON))
if (btEqual(u, SIMD_EPSILON))
u = 0.0;
else if (u > 0.0f)
u = SimdSqrt(u);
u = btSqrt(u);
else
return 0;
if (SimdEqual(v, SIMD_EPSILON))
if (btEqual(v, SIMD_EPSILON))
v = 0.0;
else if (v > 0.0f)
v = SimdSqrt(v);
v = btSqrt(v);
else
return 0;
@@ -343,7 +343,7 @@ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, S
m_roots[i] -= a;
}
w += 2.0f *u;
SimdScalar tmp[2];
btScalar tmp[2];
tmp[0] = m_roots[0];
tmp[1] = m_roots[1];

12
Extras/AlgebraicCCD/BU_AlgebraicPolynomialSolver.h
View File

@@ -26,19 +26,19 @@ class BU_AlgebraicPolynomialSolver : public BUM_PolynomialSolverInterface
public:
BU_AlgebraicPolynomialSolver() {};
int Solve2Quadratic(SimdScalar p, SimdScalar q);
int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c);
int Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c);
int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d);
int Solve2Quadratic(btScalar p, btScalar q);
int Solve2QuadraticFull(btScalar a,btScalar b, btScalar c);
int Solve3Cubic(btScalar lead, btScalar a, btScalar b, btScalar c);
int Solve4Quartic(btScalar lead, btScalar a, btScalar b, btScalar c, btScalar d);
SimdScalar GetRoot(int i) const
btScalar GetRoot(int i) const
{
return m_roots[i];
}
private:
SimdScalar m_roots[4];
btScalar m_roots[4];
};

4
Extras/AlgebraicCCD/BU_Collidable.cpp
View File

@@ -16,10 +16,10 @@ subject to the following restrictions:
#include "BU_Collidable.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include <LinearMath/SimdTransform.h>
#include <LinearMath/btTransform.h>
#include "BU_MotionStateInterface.h"
BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape,void* userPointer )
BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,btPolyhedralConvexShape& shape,void* userPointer )
:m_motionState(motion),m_shape(shape),m_userPointer(userPointer)
{
}

10
Extras/AlgebraicCCD/BU_Collidable.h
View File

@@ -18,14 +18,14 @@ subject to the following restrictions:
#define BU_COLLIDABLE
class PolyhedralConvexShape;
class btPolyhedralConvexShape;
class BU_MotionStateInterface;
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/btPoint3.h>
class BU_Collidable
{
public:
BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape, void* userPointer);
BU_Collidable(BU_MotionStateInterface& motion,btPolyhedralConvexShape& shape, void* userPointer);
void* GetUserPointer() const
{
@@ -41,7 +41,7 @@ public:
return m_motionState;
}
inline const PolyhedralConvexShape& GetShape() const
inline const btPolyhedralConvexShape& GetShape() const
{
return m_shape;
};
@@ -49,7 +49,7 @@ public:
private:
BU_MotionStateInterface& m_motionState;
PolyhedralConvexShape& m_shape;
btPolyhedralConvexShape& m_shape;
void* m_userPointer;
};

160
Extras/AlgebraicCCD/BU_CollisionPair.cpp
View File

@@ -23,13 +23,13 @@ subject to the following restrictions:
#include "BU_MotionStateInterface.h"
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
#include <SimdMinMax.h>
#include "LinearMath/SimdTransformUtil.h"
#include <btSimdMinMax.h>
#include "LinearMath/btTransformUtil.h"
BU_CollisionPair::BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance)
: m_convexA(convexA),m_convexB(convexB),m_screwing(SimdVector3(0,0,0),SimdVector3(0,0,0)),
BU_CollisionPair::BU_CollisionPair(const btPolyhedralConvexShape* convexA,const btPolyhedralConvexShape* convexB,btScalar tolerance)
: m_convexA(convexA),m_convexB(convexB),m_screwing(btVector3(0,0,0),btVector3(0,0,0)),
m_tolerance(tolerance)
{
@@ -40,55 +40,55 @@ m_tolerance(tolerance)
/*
bool BU_CollisionPair::GetTimeOfImpact(const SimdVector3& linearMotionA,const SimdQuaternion& angularMotionA,const SimdVector3& linearMotionB,const SimdQuaternion& angularMotionB, SimdScalar& toi,SimdTransform& impactTransA,SimdTransform& impactTransB)
bool BU_CollisionPair::GetTimeOfImpact(const btVector3& linearMotionA,const btQuaternion& angularMotionA,const btVector3& linearMotionB,const btQuaternion& angularMotionB, btScalar& toi,btTransform& impactTransA,btTransform& impactTransB)
*/
bool BU_CollisionPair::calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
{
SimdVector3 linvelA,angvelA;
SimdVector3 linvelB,angvelB;
btVector3 linvelA,angvelA;
btVector3 linvelB,angvelB;
SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);
btTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
btTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);
SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
SimdQuaternion angularMotionA(0,0,0,1.f);
SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
SimdQuaternion angularMotionB(0,0,0,1);
btVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
btQuaternion angularMotionA(0,0,0,1.f);
btVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
btQuaternion angularMotionB(0,0,0,1);
result.m_fraction = 1.f;
SimdTransform impactTransA;
SimdTransform impactTransB;
btTransform impactTransA;
btTransform impactTransB;
int index=0;
SimdScalar toiUnscaled=result.m_fraction;
const SimdScalar toiUnscaledLimit = result.m_fraction;
btScalar toiUnscaled=result.m_fraction;
const btScalar toiUnscaledLimit = result.m_fraction;
SimdTransform a2w;
btTransform a2w;
a2w = fromA;
SimdTransform b2w = fromB;
btTransform b2w = fromB;
/* debugging code
{
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 pt;
btPoint3 pt;
m_convexB->GetVertex(v,pt);
pt = b2w * pt;
char buf[1000];
@@ -110,52 +110,52 @@ bool BU_CollisionPair::calcTimeOfImpact(
*/
SimdTransform b2wp = b2w;
btTransform b2wp = b2w;
b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
b2wp.setRotation( b2w.getRotation() + angularMotionB);
impactTransB = b2wp;
SimdTransform a2wp;
btTransform a2wp;
a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
a2wp.setRotation(a2w.getRotation()+angularMotionA);
impactTransA = a2wp;
SimdTransform a2winv;
btTransform a2winv;
a2winv = a2w.inverse();
SimdTransform b2wpinv;
btTransform b2wpinv;
b2wpinv = b2wp.inverse();
SimdTransform b2winv;
btTransform b2winv;
b2winv = b2w.inverse();
SimdTransform a2wpinv;
btTransform a2wpinv;
a2wpinv = a2wp.inverse();
//Redon's version with concatenated transforms
SimdTransform relative;
btTransform relative;
relative = b2w * b2wpinv * a2wp * a2winv;
//relative = a2winv * a2wp * b2wpinv * b2w;
SimdQuaternion qrel;
btQuaternion qrel;
relative.getBasis().getRotation(qrel);
SimdVector3 linvel = relative.getOrigin();
btVector3 linvel = relative.getOrigin();
if (linvel.length() < SCREWEPSILON)
{
linvel.setValue(0.,0.,0.);
}
SimdVector3 angvel;
angvel[0] = 2.f * SimdAsin (qrel[0]);
angvel[1] = 2.f * SimdAsin (qrel[1]);
angvel[2] = 2.f * SimdAsin (qrel[2]);
btVector3 angvel;
angvel[0] = 2.f * btAsin (qrel[0]);
angvel[1] = 2.f * btAsin (qrel[1]);
angvel[2] = 2.f * btAsin (qrel[2]);
if (angvel.length() < SCREWEPSILON)
{
@@ -165,10 +165,10 @@ bool BU_CollisionPair::calcTimeOfImpact(
//Redon's version with concatenated transforms
m_screwing = BU_Screwing(linvel,angvel);
SimdTransform w2s;
btTransform w2s;
m_screwing.LocalMatrix(w2s);
SimdTransform s2w;
btTransform s2w;
s2w = w2s.inverse();
//impactTransA = a2w;
@@ -176,7 +176,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
bool hit = false;
if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW()))
if (btFuzzyZero(m_screwing.GetS()) && btFuzzyZero(m_screwing.GetW()))
{
//W = 0 , S = 0 , no collision
//toi = 0;
@@ -185,7 +185,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 pt;
btPoint3 pt;
m_convexB->GetVertex(v,pt);
pt = impactTransB * pt;
char buf[1000];
@@ -217,7 +217,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
//for all edged in A check agains all edges in B
for (int ea = 0;ea < m_convexA->GetNumEdges();ea++)
{
SimdPoint3 pA0,pA1;
btPoint3 pA0,pA1;
m_convexA->GetEdge(ea,pA0,pA1);
@@ -231,7 +231,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
for (int eb = 0; eb < numedgesB;eb++)
{
{
SimdPoint3 pB0,pB1;
btPoint3 pB0,pB1;
m_convexB->GetEdge(eb,pB0,pB1);
pB0= b2w * pB0;//in world space
@@ -241,12 +241,12 @@ bool BU_CollisionPair::calcTimeOfImpact(
pB1 = w2s * pB1;//in screwing space
SimdScalar lambda,mu;
btScalar lambda,mu;
toiUnscaled = 1.;
SimdVector3 edgeDirA(pA1-pA0);
SimdVector3 edgeDirB(pB1-pB0);
btVector3 edgeDirA(pA1-pA0);
btVector3 edgeDirB(pB1-pB0);
if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu))
{
@@ -258,10 +258,10 @@ bool BU_CollisionPair::calcTimeOfImpact(
//inside check is already done by checking the mu and gamma !
SimdPoint3 vtx = pA0+lambda * (pA1-pA0);
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
btPoint3 vtx = pA0+lambda * (pA1-pA0);
btPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
SimdPoint3 hitptWorld = s2w * hitpt;
btPoint3 hitptWorld = s2w * hitpt;
{
if (toiUnscaled < result.m_fraction)
@@ -269,7 +269,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
hit = true;
SimdVector3 hitNormal = edgeDirB.cross(edgeDirA);
btVector3 hitNormal = edgeDirB.cross(edgeDirA);
hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled);
@@ -279,9 +279,9 @@ bool BU_CollisionPair::calcTimeOfImpact(
//an approximated normal can be calculated by taking the cross product of both edges
//take care of the sign !
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld);
btScalar dist = m_screwing.GetU().dot(hitNormalWorld);
if (dist > 0)
hitNormalWorld *= -1;
@@ -312,7 +312,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
//int v=3;
{
SimdPoint3 vtx;
btPoint3 vtx;
m_convexA->GetVertex(v,vtx);
vtx = a2w * vtx;//in world space
@@ -326,23 +326,23 @@ bool BU_CollisionPair::calcTimeOfImpact(
{
SimdVector3 planeNorm;
SimdPoint3 planeSupport;
btVector3 planeNorm;
btPoint3 planeSupport;
m_convexB->GetPlane(planeNorm,planeSupport,p);
planeSupport = b2w * planeSupport;//transform to world space
SimdVector3 planeNormWorld = b2w.getBasis() * planeNorm;
btVector3 planeNormWorld = b2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld;
planeNorm.normalize();
SimdScalar d = planeSupport.dot(planeNorm);
btScalar d = planeSupport.dot(planeNorm);
SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
btVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
BU_VertexPoly vtxApolyB;
@@ -368,11 +368,11 @@ bool BU_CollisionPair::calcTimeOfImpact(
{
// printf("toiUnscaled %f\n",toiUnscaled );
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
btPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
btVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdPoint3 hitptWorld = s2w * hitpt;
btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
btPoint3 hitptWorld = s2w * hitpt;
hitpt = b2winv * hitptWorld;
@@ -408,7 +408,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
//int v=0;
{
SimdPoint3 vtx;
btPoint3 vtx;
m_convexB->GetVertex(v,vtx);
vtx = b2w * vtx;//in world space
@@ -436,23 +436,23 @@ bool BU_CollisionPair::calcTimeOfImpact(
{
{
SimdVector3 planeNorm;
SimdPoint3 planeSupport;
btVector3 planeNorm;
btPoint3 planeSupport;
m_convexA->GetPlane(planeNorm,planeSupport,p);
planeSupport = a2w * planeSupport;//transform to world space
SimdVector3 planeNormWorld = a2w.getBasis() * planeNorm;
btVector3 planeNormWorld = a2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld;
planeNorm.normalize();
SimdScalar d = planeSupport.dot(planeNorm);
btScalar d = planeSupport.dot(planeNorm);
SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
btVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
BU_VertexPoly vtxBpolyA;
@@ -464,15 +464,15 @@ bool BU_CollisionPair::calcTimeOfImpact(
{
if (toiUnscaled < toiUnscaledLimit)
{
SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
//SimdScalar len = hitNormal.length()-1;
btPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
btVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
//btScalar len = hitNormal.length()-1;
//assert( SimdFuzzyZero(len) );
//assert( btFuzzyZero(len) );
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdPoint3 hitptWorld = s2w * hitpt;
btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
btPoint3 hitptWorld = s2w * hitpt;
hitpt = a2winv * hitptWorld;
@@ -522,19 +522,19 @@ bool BU_CollisionPair::calcTimeOfImpact(
} else
{
//SimdScalar vel = linearMotionB.length();
//btScalar vel = linearMotionB.length();
//todo: check this margin
result.m_fraction *= 0.99f;
//move B to new position
impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB);
SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
btQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
ornB.normalize();
impactTransB.setRotation(ornB);
//now transform A
SimdTransform a2s,a2b;
btTransform a2s,a2b;
a2s.mult( w2s , a2w);
a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
a2s.multInverseLeft(w2s,a2s);
@@ -543,10 +543,10 @@ bool BU_CollisionPair::calcTimeOfImpact(
//transform by motion B
impactTransA.mult(impactTransB, a2b);
//normalize rotation
SimdQuaternion orn;
btQuaternion orn;
impactTransA.getBasis().getRotation(orn);
orn.normalize();
impactTransA.setBasis(SimdMatrix3x3(orn));
impactTransA.setBasis(btMatrix3x3(orn));
}
}
@@ -555,7 +555,7 @@ bool BU_CollisionPair::calcTimeOfImpact(
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 pt;
btPoint3 pt;
m_convexB->GetVertex(v,pt);
pt = impactTransB * pt;
char buf[1000];

22
Extras/AlgebraicCCD/BU_CollisionPair.h
View File

@@ -21,34 +21,34 @@ subject to the following restrictions:
#include <NarrowPhaseCollision/ConvexCast.h>
#include <SimdQuaternion.h>
#include <btQuaternion.h>
class PolyhedralConvexShape;
class btPolyhedralConvexShape;
///BU_CollisionPair implements collision algorithm for algebraic time of impact calculation of feature based shapes.
class BU_CollisionPair : public ConvexCast
class BU_CollisionPair : public btConvexCast
{
public:
BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance=0.2f);
BU_CollisionPair(const btPolyhedralConvexShape* convexA,const btPolyhedralConvexShape* convexB,btScalar tolerance=0.2f);
//toi
virtual bool calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result);
private:
const PolyhedralConvexShape* m_convexA;
const PolyhedralConvexShape* m_convexB;
const btPolyhedralConvexShape* m_convexA;
const btPolyhedralConvexShape* m_convexB;
BU_Screwing m_screwing;
SimdScalar m_tolerance;
btScalar m_tolerance;
};
#endif //BU_COLLISIONPAIR

238
Extras/AlgebraicCCD/BU_EdgeEdge.cpp
View File

@@ -16,8 +16,8 @@ subject to the following restrictions:
#include "BU_EdgeEdge.h"
#include "BU_Screwing.h"
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btPoint3.h>
//#include "BU_IntervalArithmeticPolynomialSolver.h"
#include "BU_AlgebraicPolynomialSolver.h"
@@ -36,37 +36,37 @@ BU_EdgeEdge::BU_EdgeEdge()
bool BU_EdgeEdge::GetTimeOfImpact(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lambda1,
SimdScalar& mu1
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lambda1,
btScalar& mu1
)
{
bool hit=false;
SimdScalar lambda;
SimdScalar mu;
btScalar lambda;
btScalar mu;
const SimdScalar w=screwAB.GetW();
const SimdScalar s=screwAB.GetS();
const btScalar w=screwAB.GetW();
const btScalar s=screwAB.GetS();
if (SimdFuzzyZero(s) &&
SimdFuzzyZero(w))
if (btFuzzyZero(s) &&
btFuzzyZero(w))
{
//no motion, no collision
return false;
}
if (SimdFuzzyZero(w) )
if (btFuzzyZero(w) )
{
//pure translation W=0, S <> 0
//no trig, f(t)=t
SimdScalar det = u.y()*v.x()-u.x()*v.y();
if (!SimdFuzzyZero(det))
btScalar det = u.y()*v.x()-u.x()*v.y();
if (!btFuzzyZero(det))
{
lambda = (a.x()*v.y() - c.x() * v.y() - v.x() * a.y() + v.x() * c.y()) / det;
mu = (u.y() * a.x() - u.y() * c.x() - u.x() * a.y() + u.x() * c.y()) / det;
@@ -74,7 +74,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
if (mu >=0 && mu <= 1 && lambda >= 0 && lambda <= 1)
{
// single potential collision is
SimdScalar t = (c.z()-a.z()+mu*v.z()-lambda*u.z())/s;
btScalar t = (c.z()-a.z()+mu*v.z()-lambda*u.z())/s;
//if this is on the edge, and time t within [0..1] report hit
if (t>=0 && t <= minTime)
{
@@ -91,14 +91,14 @@ bool BU_EdgeEdge::GetTimeOfImpact(
}
} else
{
if (SimdFuzzyZero(s) )
if (btFuzzyZero(s) )
{
if (SimdFuzzyZero(u.z()) )
if (btFuzzyZero(u.z()) )
{
if (SimdFuzzyZero(v.z()) )
if (btFuzzyZero(v.z()) )
{
//u.z()=0,v.z()=0
if (SimdFuzzyZero(a.z()-c.z()))
if (btFuzzyZero(a.z()-c.z()))
{
//printf("NOT YET planar problem, 4 vertex=edge cases\n");
@@ -110,7 +110,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
} else
{
SimdScalar mu = (a.z() - c.z())/v.z();
btScalar mu = (a.z() - c.z())/v.z();
if (0<=mu && mu <= 1)
{
// printf("NOT YET//u.z()=0,v.z()<>0\n");
@@ -124,22 +124,22 @@ bool BU_EdgeEdge::GetTimeOfImpact(
{
//u.z()<>0
if (SimdFuzzyZero(v.z()) )
if (btFuzzyZero(v.z()) )
{
//printf("u.z()<>0,v.z()=0\n");
lambda = (c.z() - a.z())/u.z();
if (0<=lambda && lambda <= 1)
{
//printf("u.z()<>0,v.z()=0\n");
SimdPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f);
SimdScalar r2 = rotPt.length2();//px*px + py*py;
btPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f);
btScalar r2 = rotPt.length2();//px*px + py*py;
//either y=a*x+b, or x = a*x+b...
//depends on whether value v.x() is zero or not
SimdScalar aa;
SimdScalar bb;
btScalar aa;
btScalar bb;
if (SimdFuzzyZero(v.x()))
if (btFuzzyZero(v.x()))
{
aa = v.x()/v.y();
bb= c.x()+ (-c.y() /v.y()) *v.x();
@@ -155,21 +155,21 @@ bool BU_EdgeEdge::GetTimeOfImpact(
bb= c.y()+ (-c.x() /v.x()) *v.y();
}
SimdScalar disc = aa*aa*r2 + r2 - bb*bb;
btScalar disc = aa*aa*r2 + r2 - bb*bb;
if (disc <0)
{
//edge doesn't intersect the circle (motion of the vertex)
return false;
}
SimdScalar rad = SimdSqrt(r2);
btScalar rad = btSqrt(r2);
if (SimdFuzzyZero(disc))
if (btFuzzyZero(disc))
{
SimdPoint3 intersectPt;
btPoint3 intersectPt;
SimdScalar mu;
btScalar mu;
//intersectionPoint edge with circle;
if (SimdFuzzyZero(v.x()))
if (btFuzzyZero(v.x()))
{
intersectPt.setY( (-2*aa*bb)/(2*(aa*aa+1)));
intersectPt.setX( aa*intersectPt.y()+bb );
@@ -191,20 +191,20 @@ bool BU_EdgeEdge::GetTimeOfImpact(
{
//two points...
//intersectionPoint edge with circle;
SimdPoint3 intersectPt;
btPoint3 intersectPt;
//intersectionPoint edge with circle;
if (SimdFuzzyZero(v.x()))
if (btFuzzyZero(v.x()))
{
SimdScalar mu;
btScalar mu;
intersectPt.setY((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
intersectPt.setY((-2.f*aa*bb+2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
intersectPt.setX(aa*intersectPt.y()+bb);
mu = ((intersectPt.getY()-c.getY())/v.getY());
if (0.f <= mu && mu <= 1.f)
{
hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
}
intersectPt.setY((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
intersectPt.setY((-2.f*aa*bb-2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
intersectPt.setX(aa*intersectPt.y()+bb);
mu = ((intersectPt.getY()-c.getY())/v.getY());
if (0 <= mu && mu <= 1)
@@ -214,16 +214,16 @@ bool BU_EdgeEdge::GetTimeOfImpact(
} else
{
SimdScalar mu;
btScalar mu;
intersectPt.setX((-2.f*aa*bb+2.f*SimdSqrt(disc))/(2*(aa*aa+1.f)));
intersectPt.setX((-2.f*aa*bb+2.f*btSqrt(disc))/(2*(aa*aa+1.f)));
intersectPt.setY(aa*intersectPt.x()+bb);
mu = ((intersectPt.getX()-c.getX())/v.getX());
if (0 <= mu && mu <= 1)
{
hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
}
intersectPt.setX((-2.f*aa*bb-2.f*SimdSqrt(disc))/(2.f*(aa*aa+1.f)));
intersectPt.setX((-2.f*aa*bb-2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
intersectPt.setY(aa*intersectPt.x()+bb);
mu = ((intersectPt.getX()-c.getX())/v.getX());
if (0.f <= mu && mu <= 1.f)
@@ -247,7 +247,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
{
//u.z()<>0,v.z()<>0
//printf("general case with s=0\n");
hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
hit = GetTimeOfImpactbteralCase(screwAB,a,u,c,v,minTime,lambda,mu);
if (hit)
{
lambda1 = lambda;
@@ -260,7 +260,7 @@ bool BU_EdgeEdge::GetTimeOfImpact(
} else
{
//printf("general case, W<>0,S<>0\n");
hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
hit = GetTimeOfImpactbteralCase(screwAB,a,u,c,v,minTime,lambda,mu);
if (hit)
{
lambda1 = lambda;
@@ -277,60 +277,60 @@ bool BU_EdgeEdge::GetTimeOfImpact(
}
bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
bool BU_EdgeEdge::GetTimeOfImpactbteralCase(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lambda,
SimdScalar& mu
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lambda,
btScalar& mu
)
{
bool hit = false;
SimdScalar coefs[4]={0.f,0.f,0.f,0.f};
btScalar coefs[4]={0.f,0.f,0.f,0.f};
BU_Polynomial polynomialSolver;
int numroots = 0;
//SimdScalar eps=1e-15f;
//SimdScalar eps2=1e-20f;
SimdScalar s=screwAB.GetS();
SimdScalar w = screwAB.GetW();
//btScalar eps=1e-15f;
//btScalar eps2=1e-20f;
btScalar s=screwAB.GetS();
btScalar w = screwAB.GetW();
SimdScalar ax = a.x();
SimdScalar ay = a.y();
SimdScalar az = a.z();
SimdScalar cx = c.x();
SimdScalar cy = c.y();
SimdScalar cz = c.z();
SimdScalar vx = v.x();
SimdScalar vy = v.y();
SimdScalar vz = v.z();
SimdScalar ux = u.x();
SimdScalar uy = u.y();
SimdScalar uz = u.z();
btScalar ax = a.x();
btScalar ay = a.y();
btScalar az = a.z();
btScalar cx = c.x();
btScalar cy = c.y();
btScalar cz = c.z();
btScalar vx = v.x();
btScalar vy = v.y();
btScalar vz = v.z();
btScalar ux = u.x();
btScalar uy = u.y();
btScalar uz = u.z();
if (!SimdFuzzyZero(v.z()))
if (!btFuzzyZero(v.z()))
{
//Maple Autogenerated C code
SimdScalar t1,t2,t3,t4,t7,t8,t10;
SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
SimdScalar t31,t32,t33,t34,t35,t36,t39,t40;
SimdScalar t41,t43,t48;
SimdScalar t63;
btScalar t1,t2,t3,t4,t7,t8,t10;
btScalar t13,t14,t15,t16,t17,t18,t19,t20;
btScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
btScalar t31,t32,t33,t34,t35,t36,t39,t40;
btScalar t41,t43,t48;
btScalar t63;
SimdScalar aa,bb,cc,dd;//the coefficients
btScalar aa,bb,cc,dd;//the coefficients
t1 = v.y()*s; t2 = t1*u.x();
t3 = v.x()*s;
t4 = t3*u.y();
t7 = SimdTan(w/2.0f);
t7 = btTan(w/2.0f);
t8 = 1.0f/t7;
t10 = 1.0f/v.z();
aa = (t2-t4)*t8*t10;
@@ -377,17 +377,17 @@ bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
} else
{
SimdScalar t1,t2,t3,t4,t7,t8,t10;
SimdScalar t13,t14,t15,t16,t17,t18,t19,t20;
SimdScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
SimdScalar t31,t32,t33,t34,t35,t36,t37,t38,t57;
SimdScalar p1,p2,p3,p4;
btScalar t1,t2,t3,t4,t7,t8,t10;
btScalar t13,t14,t15,t16,t17,t18,t19,t20;
btScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
btScalar t31,t32,t33,t34,t35,t36,t37,t38,t57;
btScalar p1,p2,p3,p4;
t1 = uy*s;
t2 = t1*vx;
t3 = ux*s;
t4 = t3*vy;
t7 = SimdTan(w/2.0f);
t7 = btTan(w/2.0f);
t8 = 1/t7;
t10 = 1/uz;
t13 = ux*az;
@@ -435,38 +435,38 @@ bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
for (int i=0;i<numroots;i++)
{
//SimdScalar tau = roots[i];//polynomialSolver.GetRoot(i);
SimdScalar tau = polynomialSolver.GetRoot(i);
//btScalar tau = roots[i];//polynomialSolver.GetRoot(i);
btScalar tau = polynomialSolver.GetRoot(i);
//check whether mu and lambda are in range [0..1]
if (!SimdFuzzyZero(v.z()))
if (!btFuzzyZero(v.z()))
{
SimdScalar A1=(ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz);
SimdScalar B1=((1.f+tau*tau)*(cx*SimdTan(1.f/2.f*w)*vz+
vx*az*SimdTan(1.f/2.f*w)-vx*cz*SimdTan(1.f/2.f*w)+
vx*s*tau)/SimdTan(1.f/2.f*w)/vz)-(ax-ax*tau*tau-2.f*tau*ay);
btScalar A1=(ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz);
btScalar B1=((1.f+tau*tau)*(cx*btTan(1.f/2.f*w)*vz+
vx*az*btTan(1.f/2.f*w)-vx*cz*btTan(1.f/2.f*w)+
vx*s*tau)/btTan(1.f/2.f*w)/vz)-(ax-ax*tau*tau-2.f*tau*ay);
lambda = B1/A1;
mu = (a.z()-c.z()+lambda*u.z()+(s*tau)/(SimdTan(w/2.f)))/v.z();
mu = (a.z()-c.z()+lambda*u.z()+(s*tau)/(btTan(w/2.f)))/v.z();
//double check in original equation
SimdScalar lhs = (a.x()+lambda*u.x())
btScalar lhs = (a.x()+lambda*u.x())
*((1.f-tau*tau)/(1.f+tau*tau))-
(a.y()+lambda*u.y())*((2.f*tau)/(1.f+tau*tau));
lhs = lambda*((ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz));
SimdScalar rhs = c.x()+mu*v.x();
btScalar rhs = c.x()+mu*v.x();
rhs = ((1.f+tau*tau)*(cx*SimdTan(1.f/2.f*w)*vz+vx*az*SimdTan(1.f/2.f*w)-
vx*cz*SimdTan(1.f/2.f*w)+vx*s*tau)/(SimdTan(1.f/2.f*w)*vz))-
rhs = ((1.f+tau*tau)*(cx*btTan(1.f/2.f*w)*vz+vx*az*btTan(1.f/2.f*w)-
vx*cz*btTan(1.f/2.f*w)+vx*s*tau)/(btTan(1.f/2.f*w)*vz))-
(ax-ax*tau*tau-2.f*tau*ay);
/*SimdScalar res = coefs[0]*tau*tau*tau+
/*btScalar res = coefs[0]*tau*tau*tau+
coefs[1]*tau*tau+
coefs[2]*tau+
coefs[3];*/
@@ -484,7 +484,7 @@ bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
}
SimdScalar t = 2.f*SimdAtan(tau)/screwAB.GetW();
btScalar t = 2.f*btAtan(tau)/screwAB.GetW();
//tau = tan (wt/2) so 2*atan (tau)/w
if (t>=0.f && t<minTime)
{
@@ -517,24 +517,24 @@ bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
//C -S
//S C
bool BU_EdgeEdge::Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime)
bool BU_EdgeEdge::Calc2DRotationPointPoint(const btPoint3& rotPt, btScalar rotRadius, btScalar rotW,const btPoint3& intersectPt,btScalar& minTime)
{
bool hit = false;
// now calculate the planeEquation for the vertex motion,
// and check if the intersectionpoint is at the positive side
SimdPoint3 rotPt1(SimdCos(rotW)*rotPt.x()-SimdSin(rotW)*rotPt.y(),
SimdSin(rotW)*rotPt.x()+SimdCos(rotW)*rotPt.y(),
btPoint3 rotPt1(btCos(rotW)*rotPt.x()-btSin(rotW)*rotPt.y(),
btSin(rotW)*rotPt.x()+btCos(rotW)*rotPt.y(),
0.f);
SimdVector3 rotVec = rotPt1-rotPt;
btVector3 rotVec = rotPt1-rotPt;
SimdVector3 planeNormal( -rotVec.y() , rotVec.x() ,0.f);
btVector3 planeNormal( -rotVec.y() , rotVec.x() ,0.f);
//SimdPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly
SimdScalar planeD = planeNormal.dot(rotPt1);
//btPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly
btScalar planeD = planeNormal.dot(rotPt1);
SimdScalar dist = (planeNormal.dot(intersectPt)-planeD);
btScalar dist = (planeNormal.dot(intersectPt)-planeD);
hit = (dist >= -0.001);
//if (hit)
@@ -545,10 +545,10 @@ bool BU_EdgeEdge::Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar r
// cos (alpha) = adjacent/hypothenuse;
//adjacent = dotproduct(ipedge,point);
//hypothenuse = sqrt(r2);
SimdScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
SimdScalar hypo = rotRadius;
SimdScalar alpha = SimdAcos(adjacent/hypo);
SimdScalar t = alpha / rotW;
btScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
btScalar hypo = rotRadius;
btScalar alpha = btAcos(adjacent/hypo);
btScalar t = alpha / rotW;
if (t >= 0 && t < minTime)
{
hit = true;
@@ -564,13 +564,13 @@ bool BU_EdgeEdge::Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar r
bool BU_EdgeEdge::GetTimeOfImpactVertexEdge(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lamda,
SimdScalar& mu
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lamda,
btScalar& mu
)
{

54
Extras/AlgebraicCCD/BU_EdgeEdge.h
View File

@@ -18,13 +18,13 @@ subject to the following restrictions:
#define BU_EDGEEDGE
class BU_Screwing;
#include <LinearMath/SimdTransform.h>
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/SimdVector3.h>
#include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btVector3.h>
//class BUM_Point2;
#include <LinearMath/SimdScalar.h>
#include <LinearMath/btScalar.h>
///BU_EdgeEdge implements algebraic time of impact calculation between two (angular + linear) moving edges.
class BU_EdgeEdge
@@ -35,39 +35,39 @@ public:
BU_EdgeEdge();
bool GetTimeOfImpact(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lamda,
SimdScalar& mu
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lamda,
btScalar& mu
);
private:
bool Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime);
bool GetTimeOfImpactGeneralCase(
bool Calc2DRotationPointPoint(const btPoint3& rotPt, btScalar rotRadius, btScalar rotW,const btPoint3& intersectPt,btScalar& minTime);
bool GetTimeOfImpactbteralCase(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lamda,
SimdScalar& mu
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lamda,
btScalar& mu
);
bool GetTimeOfImpactVertexEdge(
const BU_Screwing& screwAB,
const SimdPoint3& a,//edge in object A
const SimdVector3& u,
const SimdPoint3& c,//edge in object B
const SimdVector3& v,
SimdScalar &minTime,
SimdScalar &lamda,
SimdScalar& mu
const btPoint3& a,//edge in object A
const btVector3& u,
const btPoint3& c,//edge in object B
const btVector3& v,
btScalar &minTime,
btScalar &lamda,
btScalar& mu
);

30
Extras/AlgebraicCCD/BU_MotionStateInterface.h
View File

@@ -18,32 +18,32 @@ subject to the following restrictions:
#define BU_MOTIONSTATE
#include <LinearMath/SimdTransform.h>
#include <LinearMath/SimdPoint3.h>
#include <SimdQuaternion.h>
#include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h>
#include <btQuaternion.h>
class BU_MotionStateInterface
{
public:
virtual ~BU_MotionStateInterface(){};
virtual void SetTransform(const SimdTransform& trans) = 0;
virtual void GetTransform(SimdTransform& trans) const = 0;
virtual void SetTransform(const btTransform& trans) = 0;
virtual void GetTransform(btTransform& trans) const = 0;
virtual void SetPosition(const SimdPoint3& position) = 0;
virtual void GetPosition(SimdPoint3& position) const = 0;
virtual void SetPosition(const btPoint3& position) = 0;
virtual void GetPosition(btPoint3& position) const = 0;
virtual void SetOrientation(const SimdQuaternion& orientation) = 0;
virtual void GetOrientation(SimdQuaternion& orientation) const = 0;
virtual void SetOrientation(const btQuaternion& orientation) = 0;
virtual void GetOrientation(btQuaternion& orientation) const = 0;
virtual void SetBasis(const SimdMatrix3x3& basis) = 0;
virtual void GetBasis(SimdMatrix3x3& basis) const = 0;
virtual void SetBasis(const btMatrix3x3& basis) = 0;
virtual void GetBasis(btMatrix3x3& basis) const = 0;
virtual void SetLinearVelocity(const SimdVector3& linvel) = 0;
virtual void GetLinearVelocity(SimdVector3& linvel) const = 0;
virtual void SetLinearVelocity(const btVector3& linvel) = 0;
virtual void GetLinearVelocity(btVector3& linvel) const = 0;
virtual void GetAngularVelocity(SimdVector3& angvel) const = 0;
virtual void SetAngularVelocity(const SimdVector3& angvel) = 0;
virtual void GetAngularVelocity(btVector3& angvel) const = 0;
virtual void SetAngularVelocity(const btVector3& angvel) = 0;
};

10
Extras/AlgebraicCCD/BU_PolynomialSolverInterface.h
View File

@@ -15,7 +15,7 @@ subject to the following restrictions:
#ifndef BUM_POLYNOMIAL_SOLVER_INTERFACE
#define BUM_POLYNOMIAL_SOLVER_INTERFACE
#include <LinearMath/SimdScalar.h>
#include <LinearMath/btScalar.h>
//
//BUM_PolynomialSolverInterface is interface class for polynomial root finding.
//The number of roots is returned as a result, query GetRoot to get the actual solution.
@@ -26,13 +26,13 @@ public:
virtual ~BUM_PolynomialSolverInterface() {};
// virtual int Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) = 0;
// virtual int Solve2QuadraticFull(btScalar a,btScalar b, btScalar c) = 0;
virtual int Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c) = 0;
virtual int Solve3Cubic(btScalar lead, btScalar a, btScalar b, btScalar c) = 0;
virtual int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d) = 0;
virtual int Solve4Quartic(btScalar lead, btScalar a, btScalar b, btScalar c, btScalar d) = 0;
virtual SimdScalar GetRoot(int i) const = 0;
virtual btScalar GetRoot(int i) const = 0;
};

90
Extras/AlgebraicCCD/BU_Screwing.cpp
View File

@@ -18,15 +18,15 @@ subject to the following restrictions:
#include "BU_Screwing.h"
BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel) {
BU_Screwing::BU_Screwing(const btVector3& relLinVel,const btVector3& relAngVel) {
const SimdScalar dx=relLinVel[0];
const SimdScalar dy=relLinVel[1];
const SimdScalar dz=relLinVel[2];
const SimdScalar wx=relAngVel[0];
const SimdScalar wy=relAngVel[1];
const SimdScalar wz=relAngVel[2];
const btScalar dx=relLinVel[0];
const btScalar dy=relLinVel[1];
const btScalar dz=relLinVel[2];
const btScalar wx=relAngVel[0];
const btScalar wy=relAngVel[1];
const btScalar wz=relAngVel[2];
// Compute the screwing parameters :
// w : total amount of rotation
@@ -34,25 +34,25 @@ BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngV
// u : vector along the screwing axis (||u||=1)
// o : point on the screwing axis
m_w=SimdSqrt(wx*wx+wy*wy+wz*wz);
m_w=btSqrt(wx*wx+wy*wy+wz*wz);
//if (!w) {
if (fabs(m_w)<SCREWEPSILON ) {
assert(m_w == 0.f);
m_w=0.;
m_s=SimdSqrt(dx*dx+dy*dy+dz*dz);
m_s=btSqrt(dx*dx+dy*dy+dz*dz);
if (fabs(m_s)<SCREWEPSILON ) {
assert(m_s == 0.);
m_s=0.;
m_u=SimdPoint3(0.,0.,1.);
m_o=SimdPoint3(0.,0.,0.);
m_u=btPoint3(0.,0.,1.);
m_o=btPoint3(0.,0.,0.);
}
else {
float t=1.f/m_s;
m_u=SimdPoint3(dx*t,dy*t,dz*t);
m_o=SimdPoint3(0.f,0.f,0.f);
m_u=btPoint3(dx*t,dy*t,dz*t);
m_o=btPoint3(0.f,0.f,0.f);
}
}
else { // there is some rotation
@@ -60,35 +60,35 @@ BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngV
// we compute u
float v(1.f/m_w);
m_u=SimdPoint3(wx*v,wy*v,wz*v); // normalization
m_u=btPoint3(wx*v,wy*v,wz*v); // normalization
// decomposition of the translation along u and one orthogonal vector
SimdPoint3 t(dx,dy,dz);
btPoint3 t(dx,dy,dz);
m_s=t.dot(m_u); // component along u
if (fabs(m_s)<SCREWEPSILON)
{
//printf("m_s component along u < SCREWEPSILION\n");
m_s=0.f;
}
SimdPoint3 n1(t-(m_s*m_u)); // the remaining part (which is orthogonal to u)
btPoint3 n1(t-(m_s*m_u)); // the remaining part (which is orthogonal to u)
// now we have to compute o
//SimdScalar len = n1.length2();
//btScalar len = n1.length2();
//(len >= BUM_EPSILON2) {
if (n1[0] || n1[1] || n1[2]) { // n1 is not the zero vector
n1.normalize();
SimdVector3 n1orth=m_u.cross(n1);
btVector3 n1orth=m_u.cross(n1);
float n2x=SimdCos(0.5f*m_w);
float n2y=SimdSin(0.5f*m_w);
float n2x=btCos(0.5f*m_w);
float n2y=btSin(0.5f*m_w);
m_o=0.5f*t.dot(n1)*(n1+n2x/n2y*n1orth);
}
else
{
m_o=SimdPoint3(0.f,0.f,0.f);
m_o=btPoint3(0.f,0.f,0.f);
}
}
@@ -99,7 +99,7 @@ BU_Screwing::BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngV
//the screwing frame :
void BU_Screwing::LocalMatrix(SimdTransform &t) const {
void BU_Screwing::LocalMatrix(btTransform &t) const {
//So the whole computations do this : align the Oz axis along the
// screwing axis (thanks to u), and then find two others orthogonal axes to
// complete the basis.
@@ -107,9 +107,9 @@ void BU_Screwing::LocalMatrix(SimdTransform &t) const {
if ((m_u[0]>SCREWEPSILON)||(m_u[0]<-SCREWEPSILON)||(m_u[1]>SCREWEPSILON)||(m_u[1]<-SCREWEPSILON))
{
// to avoid numerical problems
float n=SimdSqrt(m_u[0]*m_u[0]+m_u[1]*m_u[1]);
float n=btSqrt(m_u[0]*m_u[0]+m_u[1]*m_u[1]);
float invn=1.0f/n;
SimdMatrix3x3 mat;
btMatrix3x3 mat;
mat[0][0]=-m_u[1]*invn;
mat[0][1]=m_u[0]*invn;
@@ -123,7 +123,7 @@ void BU_Screwing::LocalMatrix(SimdTransform &t) const {
mat[2][1]=m_u[1];
mat[2][2]=m_u[2];
t.setOrigin(SimdPoint3(
t.setOrigin(btPoint3(
m_o[0]*m_u[1]*invn-m_o[1]*m_u[0]*invn,
-(m_o[0]*mat[1][0]+m_o[1]*mat[1][1]+m_o[2]*n),
-(m_o[0]*m_u[0]+m_o[1]*m_u[1]+m_o[2]*m_u[2])));
@@ -133,24 +133,24 @@ void BU_Screwing::LocalMatrix(SimdTransform &t) const {
}
else {
SimdMatrix3x3 m;
btMatrix3x3 m;
m[0][0]=1.;
m[1][0]=0.;
m[2][0]=0.;
m[0][1]=0.f;
m[1][1]=float(SimdSign(m_u[2]));
m[1][1]=float(btSign(m_u[2]));
m[2][1]=0.f;
m[0][2]=0.f;
m[1][2]=0.f;
m[2][2]=float(SimdSign(m_u[2]));
m[2][2]=float(btSign(m_u[2]));
t.setOrigin(SimdPoint3(
t.setOrigin(btPoint3(
-m_o[0],
-SimdSign(m_u[2])*m_o[1],
-SimdSign(m_u[2])*m_o[2]
-btSign(m_u[2])*m_o[1],
-btSign(m_u[2])*m_o[2]
));
t.setBasis(m);
@@ -158,42 +158,42 @@ void BU_Screwing::LocalMatrix(SimdTransform &t) const {
}
//gives interpolated transform for time in [0..1] in screwing frame
SimdTransform BU_Screwing::InBetweenTransform(const SimdTransform& tr,SimdScalar t) const
btTransform BU_Screwing::InBetweenTransform(const btTransform& tr,btScalar t) const
{
SimdPoint3 org = tr.getOrigin();
btPoint3 org = tr.getOrigin();
SimdPoint3 neworg (
org.x()*SimdCos(m_w*t)-org.y()*SimdSin(m_w*t),
org.x()*SimdSin(m_w*t)+org.y()*SimdCos(m_w*t),
btPoint3 neworg (
org.x()*btCos(m_w*t)-org.y()*btSin(m_w*t),
org.x()*btSin(m_w*t)+org.y()*btCos(m_w*t),
org.z()+m_s*CalculateF(t));
SimdTransform newtr;
btTransform newtr;
newtr.setOrigin(neworg);
SimdMatrix3x3 basis = tr.getBasis();
SimdMatrix3x3 basisorg = tr.getBasis();
btMatrix3x3 basis = tr.getBasis();
btMatrix3x3 basisorg = tr.getBasis();
SimdQuaternion rot(SimdVector3(0.,0.,1.),m_w*t);
SimdQuaternion tmpOrn;
btQuaternion rot(btVector3(0.,0.,1.),m_w*t);
btQuaternion tmpOrn;
tr.getBasis().getRotation(tmpOrn);
rot = rot * tmpOrn;
//to avoid numerical drift, normalize quaternion
rot.normalize();
newtr.setBasis(SimdMatrix3x3(rot));
newtr.setBasis(btMatrix3x3(rot));
return newtr;
}
SimdScalar BU_Screwing::CalculateF(SimdScalar t) const
btScalar BU_Screwing::CalculateF(btScalar t) const
{
SimdScalar result;
btScalar result;
if (!m_w)
{
result = t;
} else
{
result = ( SimdTan((m_w*t)/2.f) / SimdTan(m_w/2.f));
result = ( btTan((m_w*t)/2.f) / btTan(m_w/2.f));
}
return result;
}

42
Extras/AlgebraicCCD/BU_Screwing.h
View File

@@ -18,9 +18,9 @@ subject to the following restrictions:
#define B_SCREWING_H
#include <LinearMath/SimdVector3.h>
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/SimdTransform.h>
#include <LinearMath/btVector3.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btTransform.h>
#define SCREWEPSILON 0.00001f
@@ -31,47 +31,47 @@ class BU_Screwing
public:
BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel);
BU_Screwing(const btVector3& relLinVel,const btVector3& relAngVel);
~BU_Screwing() {
};
SimdScalar CalculateF(SimdScalar t) const;
btScalar CalculateF(btScalar t) const;
//gives interpolated position for time in [0..1] in screwing frame
inline SimdPoint3 InBetweenPosition(const SimdPoint3& pt,SimdScalar t) const
inline btPoint3 InBetweenPosition(const btPoint3& pt,btScalar t) const
{
return SimdPoint3(
pt.x()*SimdCos(m_w*t)-pt.y()*SimdSin(m_w*t),
pt.x()*SimdSin(m_w*t)+pt.y()*SimdCos(m_w*t),
return btPoint3(
pt.x()*btCos(m_w*t)-pt.y()*btSin(m_w*t),
pt.x()*btSin(m_w*t)+pt.y()*btCos(m_w*t),
pt.z()+m_s*CalculateF(t));
}
inline SimdVector3 InBetweenVector(const SimdVector3& vec,SimdScalar t) const
inline btVector3 InBetweenVector(const btVector3& vec,btScalar t) const
{
return SimdVector3(
vec.x()*SimdCos(m_w*t)-vec.y()*SimdSin(m_w*t),
vec.x()*SimdSin(m_w*t)+vec.y()*SimdCos(m_w*t),
return btVector3(
vec.x()*btCos(m_w*t)-vec.y()*btSin(m_w*t),
vec.x()*btSin(m_w*t)+vec.y()*btCos(m_w*t),
vec.z());
}
//gives interpolated transform for time in [0..1] in screwing frame
SimdTransform InBetweenTransform(const SimdTransform& tr,SimdScalar t) const;
btTransform InBetweenTransform(const btTransform& tr,btScalar t) const;
//gives matrix from global frame into screwing frame
void LocalMatrix(SimdTransform &t) const;
void LocalMatrix(btTransform &t) const;
inline const SimdVector3& GetU() const { return m_u;}
inline const SimdVector3& GetO() const {return m_o;}
inline const SimdScalar GetS() const{ return m_s;}
inline const SimdScalar GetW() const { return m_w;}
inline const btVector3& GetU() const { return m_u;}
inline const btVector3& GetO() const {return m_o;}
inline const btScalar GetS() const{ return m_s;}
inline const btScalar GetW() const { return m_w;}
private:
float m_w;
float m_s;
SimdVector3 m_u;
SimdVector3 m_o;
btVector3 m_u;
btVector3 m_o;
};
#endif //B_SCREWING_H

32
Extras/AlgebraicCCD/BU_StaticMotionState.h
View File

@@ -18,62 +18,62 @@ subject to the following restrictions:
#define BU_STATIC_MOTIONSTATE
#include <CollisionShapes/BU_MotionStateInterface.h>
#include <btCollisionShapes/BU_MotionStateInterface.h>
class BU_StaticMotionState :public BU_MotionStateInterface
{
public:
virtual ~BU_StaticMotionState(){};
virtual void SetTransform(const SimdTransform& trans)
virtual void SetTransform(const btTransform& trans)
{
m_trans = trans;
}
virtual void GetTransform(SimdTransform& trans) const
virtual void GetTransform(btTransform& trans) const
{
trans = m_trans;
}
virtual void SetPosition(const SimdPoint3& position)
virtual void SetPosition(const btPoint3& position)
{
m_trans.setOrigin( position );
}
virtual void GetPosition(SimdPoint3& position) const
virtual void GetPosition(btPoint3& position) const
{
position = m_trans.getOrigin();
}
virtual void SetOrientation(const SimdQuaternion& orientation)
virtual void SetOrientation(const btQuaternion& orientation)
{
m_trans.setRotation( orientation);
}
virtual void GetOrientation(SimdQuaternion& orientation) const
virtual void GetOrientation(btQuaternion& orientation) const
{
orientation = m_trans.getRotation();
}
virtual void SetBasis(const SimdMatrix3x3& basis)
virtual void SetBasis(const btMatrix3x3& basis)
{
m_trans.setBasis( basis);
}
virtual void GetBasis(SimdMatrix3x3& basis) const
virtual void GetBasis(btMatrix3x3& basis) const
{
basis = m_trans.getBasis();
}
virtual void SetLinearVelocity(const SimdVector3& linvel)
virtual void SetLinearVelocity(const btVector3& linvel)
{
m_linearVelocity = linvel;
}
virtual void GetLinearVelocity(SimdVector3& linvel) const
virtual void GetLinearVelocity(btVector3& linvel) const
{
linvel = m_linearVelocity;
}
virtual void SetAngularVelocity(const SimdVector3& angvel)
virtual void SetAngularVelocity(const btVector3& angvel)
{
m_angularVelocity = angvel;
}
virtual void GetAngularVelocity(SimdVector3& angvel) const
virtual void GetAngularVelocity(btVector3& angvel) const
{
angvel = m_angularVelocity;
}
@@ -82,9 +82,9 @@ public:
protected:
SimdTransform m_trans;
SimdVector3 m_angularVelocity;
SimdVector3 m_linearVelocity;
btTransform m_trans;
btVector3 m_angularVelocity;
btVector3 m_linearVelocity;
};

44
Extras/AlgebraicCCD/BU_VertexPoly.cpp
View File

@@ -17,9 +17,9 @@ subject to the following restrictions:
#include "BU_VertexPoly.h"
#include "BU_Screwing.h"
#include <LinearMath/SimdTransform.h>
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/SimdVector3.h>
#include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btVector3.h>
#define USE_ALGEBRAIC
#ifdef USE_ALGEBRAIC
@@ -30,7 +30,7 @@ subject to the following restrictions:
#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
#endif
inline bool TestFuzzyZero(SimdScalar x) { return SimdFabs(x) < 0.0001f; }
inline bool TestFuzzyZero(btScalar x) { return btFabs(x) < 0.0001f; }
BU_VertexPoly::BU_VertexPoly()
@@ -41,9 +41,9 @@ BU_VertexPoly::BU_VertexPoly()
//false otherwise. If true, minTime contains the time of impact
bool BU_VertexPoly::GetTimeOfImpact(
const BU_Screwing& screwAB,
const SimdPoint3& a,
const SimdVector4& planeEq,
SimdScalar &minTime,bool swapAB)
const btPoint3& a,
const btVector4& planeEq,
btScalar &minTime,bool swapAB)
{
bool hit = false;
@@ -57,21 +57,21 @@ bool BU_VertexPoly::GetTimeOfImpact(
//case w<>0 and s<> 0
const SimdScalar w=screwAB.GetW();
const SimdScalar s=screwAB.GetS();
const btScalar w=screwAB.GetW();
const btScalar s=screwAB.GetS();
SimdScalar coefs[4];
const SimdScalar p=planeEq[0];
const SimdScalar q=planeEq[1];
const SimdScalar r=planeEq[2];
const SimdScalar d=planeEq[3];
btScalar coefs[4];
const btScalar p=planeEq[0];
const btScalar q=planeEq[1];
const btScalar r=planeEq[2];
const btScalar d=planeEq[3];
const SimdVector3 norm(p,q,r);
const btVector3 norm(p,q,r);
BU_Polynomial polynomialSolver;
int numroots = 0;
//SimdScalar eps=1e-80f;
//SimdScalar eps2=1e-100f;
//btScalar eps=1e-80f;
//btScalar eps2=1e-100f;
if (TestFuzzyZero(screwAB.GetS()) )
{
@@ -93,7 +93,7 @@ bool BU_VertexPoly::GetTimeOfImpact(
// W = 0 , S <> 0
//pax+qay+r(az+st)=d
SimdScalar dist = (d - a.dot(norm));
btScalar dist = (d - a.dot(norm));
if (TestFuzzyZero(r))
{
@@ -108,7 +108,7 @@ bool BU_VertexPoly::GetTimeOfImpact(
} else
{
SimdScalar etoi = (dist)/(r*screwAB.GetS());
btScalar etoi = (dist)/(r*screwAB.GetS());
if (swapAB)
etoi *= -1;
@@ -124,7 +124,7 @@ bool BU_VertexPoly::GetTimeOfImpact(
//ax^3+bx^2+cx+d=0
//degenerate coefficients mess things up :(
SimdScalar ietsje = (r*s)/SimdTan(w/2.f);
btScalar ietsje = (r*s)/btTan(w/2.f);
if (ietsje*ietsje < 0.01f)
ietsje = 0.f;
@@ -140,9 +140,9 @@ bool BU_VertexPoly::GetTimeOfImpact(
for (int i=0;i<numroots;i++)
{
SimdScalar tau = polynomialSolver.GetRoot(i);
btScalar tau = polynomialSolver.GetRoot(i);
SimdScalar t = 2.f*SimdAtan(tau)/w;
btScalar t = 2.f*btAtan(tau)/w;
//tau = tan (wt/2) so 2*atan (tau)/w
if (swapAB)
{

12
Extras/AlgebraicCCD/BU_VertexPoly.h
View File

@@ -19,9 +19,9 @@ subject to the following restrictions:
class BU_Screwing;
#include <LinearMath/SimdTransform.h>
#include <LinearMath/SimdPoint3.h>
#include <LinearMath/SimdScalar.h>
#include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btScalar.h>
///BU_VertexPoly implements algebraic time of impact calculation between vertex and a plane.
class BU_VertexPoly
@@ -30,9 +30,9 @@ public:
BU_VertexPoly();
bool GetTimeOfImpact(
const BU_Screwing& screwAB,
const SimdPoint3& vtx,
const SimdVector4& planeEq,
SimdScalar &minTime,
const btPoint3& vtx,
const btVector4& planeEq,
btScalar &minTime,
bool swapAB);
private:

36
Extras/AlternativeCollisionAlgorithms/SphereSphereCollisionAlgorithm.cpp
View File

@@ -13,13 +13,13 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "SphereSphereCollisionAlgorithm.h"
#include "btSphereSphereCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
SphereSphereCollisionAlgorithm::SphereSphereCollisionAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
: CollisionAlgorithm(ci),
btSphereSphereCollisionAlgorithm::btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
: btCollisionAlgorithm(ci),
m_ownManifold(false),
m_manifoldPtr(mf)
{
@@ -30,7 +30,7 @@ m_manifoldPtr(mf)
}
}
SphereSphereCollisionAlgorithm::~SphereSphereCollisionAlgorithm()
btSphereSphereCollisionAlgorithm::~btSphereSphereCollisionAlgorithm()
{
if (m_ownManifold)
{
@@ -39,42 +39,42 @@ SphereSphereCollisionAlgorithm::~SphereSphereCollisionAlgorithm()
}
}
void SphereSphereCollisionAlgorithm::ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo)
void btSphereSphereCollisionAlgorithm::ProcessCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
{
if (!m_manifoldPtr)
return;
CollisionObject* col0 = static_cast<CollisionObject*>(proxy0->m_clientObject);
CollisionObject* col1 = static_cast<CollisionObject*>(proxy1->m_clientObject);
SphereShape* sphere0 = (SphereShape*)col0->m_collisionShape;
SphereShape* sphere1 = (SphereShape*)col1->m_collisionShape;
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;
SimdVector3 diff = col0->m_worldTransform.getOrigin()- col1->m_worldTransform.getOrigin();
btVector3 diff = col0->m_worldTransform.getOrigin()- col1->m_worldTransform.getOrigin();
float len = diff.length();
SimdScalar radius0 = sphere0->GetRadius();
SimdScalar radius1 = sphere1->GetRadius();
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)
SimdScalar dist = len - (radius0+radius1);
btScalar dist = len - (radius0+radius1);
SimdVector3 normalOnSurfaceB = diff / len;
btVector3 normalOnSurfaceB = diff / len;
///point on A (worldspace)
SimdVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB;
btVector3 pos0 = col0->m_worldTransform.getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
SimdVector3 pos1 = col1->m_worldTransform.getOrigin() + radius1* normalOnSurfaceB;
btVector3 pos1 = col1->m_worldTransform.getOrigin() + radius1* normalOnSurfaceB;
/// report a contact. internally this will be kept persistent, and contact reduction is done
ManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr);
btManifoldResult* resultOut = m_dispatcher->GetNewManifoldResult(col0,col1,m_manifoldPtr);
resultOut->AddContactPoint(normalOnSurfaceB,pos1,dist);
m_dispatcher->ReleaseManifoldResult(resultOut);
}
float SphereSphereCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo)
float btSphereSphereCollisionAlgorithm::CalculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo)
{
//not yet
return 1.f;

28
Extras/AlternativeCollisionAlgorithms/SphereSphereCollisionAlgorithm.h
View File

@@ -19,33 +19,33 @@ subject to the following restrictions:
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
class PersistentManifold;
class btPersistentManifold;
/// SphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
/// 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 CollisionAlgorithm
class SphereSphereCollisionAlgorithm : public CollisionAlgorithm
/// Also provides the most basic sample for custom/user btCollisionAlgorithm
class btSphereSphereCollisionAlgorithm : public btCollisionAlgorithm
{
bool m_ownManifold;
PersistentManifold* m_manifoldPtr;
btPersistentManifold* m_manifoldPtr;
public:
SphereSphereCollisionAlgorithm(const CollisionAlgorithmConstructionInfo& ci)
: CollisionAlgorithm(ci) {}
btSphereSphereCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci)
: btCollisionAlgorithm(ci) {}
virtual void ProcessCollision (BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
virtual void ProcessCollision (btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
virtual float CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,const DispatcherInfo& dispatchInfo);
virtual float CalculateTimeOfImpact(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1,const btDispatcherInfo& dispatchInfo);
SphereSphereCollisionAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1);
btSphereSphereCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1);
virtual ~SphereSphereCollisionAlgorithm();
virtual ~btSphereSphereCollisionAlgorithm();
struct CreateFunc :public CollisionAlgorithmCreateFunc
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
virtual CollisionAlgorithm* CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo& ci, BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
{
return new SphereSphereCollisionAlgorithm(0,ci,proxy0,proxy1);
return new btSphereSphereCollisionAlgorithm(0,ci,proxy0,proxy1);
}
};

136
Extras/EPA/Epa.cpp
View File

@@ -15,11 +15,11 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "LinearMath/SimdScalar.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/SimdMinMax.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btPoint3.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btSimdMinMax.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
@@ -37,11 +37,11 @@ subject to the following restrictions:
#include "NarrowPhaseCollision/EpaPolyhedron.h"
#include "NarrowPhaseCollision/Epa.h"
const SimdScalar EPA_MAX_RELATIVE_ERROR = 1e-2f;
const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD = EPA_MAX_RELATIVE_ERROR * EPA_MAX_RELATIVE_ERROR;
const btScalar EPA_MAX_RELATIVE_ERROR = 1e-2f;
const btScalar EPA_MAX_RELATIVE_ERROR_SQRD = EPA_MAX_RELATIVE_ERROR * EPA_MAX_RELATIVE_ERROR;
Epa::Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB,
const SimdTransform& transformA, const SimdTransform& transformB ) : m_pConvexShapeA( pConvexShapeA ),
Epa::Epa( btConvexShape* pConvexShapeA, btConvexShape* pConvexShapeB,
const btTransform& transformA, const btTransform& transformB ) : m_pConvexShapeA( pConvexShapeA ),
m_pConvexShapeB( pConvexShapeB ),
m_transformA( transformA ),
m_transformB( transformB )
@@ -53,14 +53,14 @@ Epa::~Epa()
{
}
bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
bool Epa::Initialize( btSimplexSolverInterface& simplexSolver )
{
// Run GJK on the enlarged shapes to obtain a simplex of the enlarged CSO
SimdVector3 v( 1, 0, 0 );
SimdScalar squaredDistance = SIMD_INFINITY;
btVector3 v( 1, 0, 0 );
btScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
btScalar delta = 0.f;
simplexSolver.reset();
@@ -70,16 +70,16 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
{
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Warning : v has zero magnitude!" );
SimdVector3 seperatingAxisInA = -v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = v * m_transformB.getBasis();
btVector3 seperatingAxisInA = -v * m_transformA.getBasis();
btVector3 seperatingAxisInB = v * m_transformB.getBasis();
SimdVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
btVector3 pInA = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
btVector3 qInB = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( pInA );
SimdPoint3 qWorld = m_transformB( qInB );
btPoint3 pWorld = m_transformA( pInA );
btPoint3 qWorld = m_transformB( qInB );
SimdVector3 w = pWorld - qWorld;
btVector3 w = pWorld - qWorld;
delta = v.dot( w );
EPA_DEBUG_ASSERT( ( delta <= 0 ) ,"Shapes are disjoint, EPA should have never been called!" );
@@ -98,7 +98,7 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
if (!closestOk)
return false;
SimdScalar prevVSqrd = squaredDistance;
btScalar prevVSqrd = squaredDistance;
squaredDistance = v.length2();
// Is v converging to v(A-B) ?
@@ -115,9 +115,9 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
++nbIterations;
}
SimdPoint3 simplexPoints[ 5 ];
SimdPoint3 wSupportPointsOnA[ 5 ];
SimdPoint3 wSupportPointsOnB[ 5 ];
btPoint3 simplexPoints[ 5 ];
btPoint3 wSupportPointsOnA[ 5 ];
btPoint3 wSupportPointsOnB[ 5 ];
int nbSimplexPoints = simplexSolver.getSimplex( wSupportPointsOnA, wSupportPointsOnB, simplexPoints );
@@ -142,18 +142,18 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
// We have a line segment inside the CSO that contains the origin
// Create an hexahedron ( two tetrahedron glued together ) by adding 3 new points
SimdVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ];
btVector3 d = simplexPoints[ 0 ] - simplexPoints[ 1 ];
d.normalize();
SimdVector3 v1;
SimdVector3 v2;
SimdVector3 v3;
btVector3 v1;
btVector3 v2;
btVector3 v3;
SimdVector3 e1;
btVector3 e1;
SimdScalar absx = abs( d.getX() );
SimdScalar absy = abs( d.getY() );
SimdScalar absz = abs( d.getZ() );
btScalar absx = abs( d.getX() );
btScalar absy = abs( d.getY() );
btScalar absz = abs( d.getZ() );
if ( absx < absy )
{
@@ -186,14 +186,14 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = v1 * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
btVector3 seperatingAxisInA = v1 * m_transformA.getBasis();
btVector3 seperatingAxisInB = -v1 * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
btVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
btVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
btPoint3 pWorld = m_transformA( p );
btPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 2 ] = pWorld;
wSupportPointsOnB[ 2 ] = qWorld;
@@ -255,21 +255,21 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
// We have a triangle inside the CSO that contains the origin
// Create an hexahedron ( two tetrahedron glued together ) by adding 2 new points
SimdVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
SimdVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
SimdVector3 triangleNormal = v0.cross( v1 );
btVector3 v0 = simplexPoints[ 2 ] - simplexPoints[ 0 ];
btVector3 v1 = simplexPoints[ 1 ] - simplexPoints[ 0 ];
btVector3 triangleNormal = v0.cross( v1 );
triangleNormal.normalize();
nbPolyhedronPoints = 5;
SimdVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
btVector3 seperatingAxisInA = triangleNormal * m_transformA.getBasis();
btVector3 seperatingAxisInB = -triangleNormal * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
btVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
btVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
btPoint3 pWorld = m_transformA( p );
btPoint3 qWorld = m_transformB( q );
wSupportPointsOnA[ 3 ] = pWorld;
wSupportPointsOnB[ 3 ] = qWorld;
@@ -316,17 +316,17 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
#endif
#ifndef EPA_POLYHEDRON_USE_PLANES
SimdPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
btPoint3 wTetraPoints[ 4 ] = { simplexPoints[ initTetraIndices[ 0 ] ],
simplexPoints[ initTetraIndices[ 1 ] ],
simplexPoints[ initTetraIndices[ 2 ] ],
simplexPoints[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
btPoint3 wTetraSupportPointsOnA[ 4 ] = { wSupportPointsOnA[ initTetraIndices[ 0 ] ],
wSupportPointsOnA[ initTetraIndices[ 1 ] ],
wSupportPointsOnA[ initTetraIndices[ 2 ] ],
wSupportPointsOnA[ initTetraIndices[ 3 ] ] };
SimdPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ],
btPoint3 wTetraSupportPointsOnB[ 4 ] = { wSupportPointsOnB[ initTetraIndices[ 0 ] ],
wSupportPointsOnB[ initTetraIndices[ 1 ] ],
wSupportPointsOnB[ initTetraIndices[ 2 ] ],
wSupportPointsOnB[ initTetraIndices[ 3 ] ] };
@@ -398,16 +398,16 @@ bool Epa::Initialize( SimplexSolverInterface& simplexSolver )
return true;
}
SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
btScalar Epa::CalcPenDepth( btPoint3& wWitnessOnA, btPoint3& wWitnessOnB )
{
SimdVector3 v;
btVector3 v;
SimdScalar upperBoundSqrd = SIMD_INFINITY;
SimdScalar vSqrd = 0;
btScalar upperBoundSqrd = SIMD_INFINITY;
btScalar vSqrd = 0;
#ifdef _DEBUG
SimdScalar prevVSqrd;
btScalar prevVSqrd;
#endif
SimdScalar delta;
btScalar delta;
bool isCloseEnough = false;
@@ -445,20 +445,20 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
#endif //_DEBUG
EPA_DEBUG_ASSERT( ( v.length2() > 0 ) ,"Zero vector not allowed!" );
SimdVector3 seperatingAxisInA = v * m_transformA.getBasis();
SimdVector3 seperatingAxisInB = -v * m_transformB.getBasis();
btVector3 seperatingAxisInA = v * m_transformA.getBasis();
btVector3 seperatingAxisInB = -v * m_transformB.getBasis();
SimdVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
SimdVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
btVector3 p = m_pConvexShapeA->LocalGetSupportingVertex( seperatingAxisInA );
btVector3 q = m_pConvexShapeB->LocalGetSupportingVertex( seperatingAxisInB );
SimdPoint3 pWorld = m_transformA( p );
SimdPoint3 qWorld = m_transformB( q );
btPoint3 pWorld = m_transformA( p );
btPoint3 qWorld = m_transformB( q );
SimdPoint3 w = pWorld - qWorld;
btPoint3 w = pWorld - qWorld;
delta = v.dot( w );
// Keep tighest upper bound
upperBoundSqrd = SimdMin( upperBoundSqrd, delta * delta / vSqrd );
upperBoundSqrd = btMin( upperBoundSqrd, delta * delta / vSqrd );
//assert_msg( vSqrd <= upperBoundSqrd, "A triangle was falsely rejected!" );
isCloseEnough = ( upperBoundSqrd <= ( 1 + 1e-4f ) * vSqrd );
@@ -544,10 +544,10 @@ SimdScalar Epa::CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
return v.length();
}
bool Epa::TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1,
const SimdPoint3& point2, const SimdPoint3& point3 )
bool Epa::TetrahedronContainsOrigin( const btPoint3& point0, const btPoint3& point1,
const btPoint3& point2, const btPoint3& point3 )
{
SimdVector3 facesNormals[ 4 ] = { ( point1 - point0 ).cross( point2 - point0 ),
btVector3 facesNormals[ 4 ] = { ( point1 - point0 ).cross( point2 - point0 ),
( point2 - point1 ).cross( point3 - point1 ),
( point3 - point2 ).cross( point0 - point2 ),
( point0 - point3 ).cross( point1 - point3 ) };
@@ -558,7 +558,7 @@ bool Epa::TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3&
( ( facesNormals[ 3 ].dot( point3 ) > 0 ) != ( facesNormals[ 3 ].dot( point2 ) > 0 ) );
}
bool Epa::TetrahedronContainsOrigin( SimdPoint3* pPoints )
bool Epa::TetrahedronContainsOrigin( btPoint3* pPoints )
{
return TetrahedronContainsOrigin( pPoints[ 0 ], pPoints[ 1 ], pPoints[ 2 ], pPoints[ 3 ] );
}

26
Extras/EPA/Epa.h
View File

@@ -21,8 +21,8 @@ subject to the following restrictions:
extern const SimdScalar EPA_MAX_RELATIVE_ERROR;
extern const SimdScalar EPA_MAX_RELATIVE_ERROR_SQRD;
extern const btScalar EPA_MAX_RELATIVE_ERROR;
extern const btScalar EPA_MAX_RELATIVE_ERROR_SQRD;
class Epa
{
@@ -34,30 +34,30 @@ class Epa
public :
Epa( ConvexShape* pConvexShapeA, ConvexShape* pConvexShapeB,
const SimdTransform& transformA, const SimdTransform& transformB );
Epa( btConvexShape* pConvexShapeA, btConvexShape* pConvexShapeB,
const btTransform& transformA, const btTransform& transformB );
~Epa();
bool Initialize( SimplexSolverInterface& simplexSolver );
bool Initialize( btSimplexSolverInterface& simplexSolver );
SimdScalar CalcPenDepth( SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB );
btScalar CalcPenDepth( btPoint3& wWitnessOnA, btPoint3& wWitnessOnB );
private :
bool TetrahedronContainsOrigin( SimdPoint3* pPoints );
bool TetrahedronContainsOrigin( const SimdPoint3& point0, const SimdPoint3& point1,
const SimdPoint3& point2, const SimdPoint3& point3 );
bool TetrahedronContainsOrigin( btPoint3* pPoints );
bool TetrahedronContainsOrigin( const btPoint3& point0, const btPoint3& point1,
const btPoint3& point2, const btPoint3& point3 );
private :
//! Priority queue
std::vector< EpaFace* > m_faceEntries;
ConvexShape* m_pConvexShapeA;
ConvexShape* m_pConvexShapeB;
btConvexShape* m_pConvexShapeA;
btConvexShape* m_pConvexShapeB;
SimdTransform m_transformA;
SimdTransform m_transformB;
btTransform m_transformA;
btTransform m_transformB;
EpaPolyhedron m_polyhedron;
};

52
Extras/EPA/EpaFace.cpp
View File

@@ -14,9 +14,9 @@ subject to the following restrictions:
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 "LinearMath/SimdScalar.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btPoint3.h"
#include "NarrowPhaseCollision/EpaCommon.h"
@@ -25,7 +25,7 @@ subject to the following restrictions:
#include "NarrowPhaseCollision/EpaFace.h"
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdScalar PLANE_THICKNESS = 1e-5f;
btScalar PLANE_THICKNESS = 1e-5f;
#endif
EpaFace::EpaFace() : m_pHalfEdge( 0 ), m_deleted( false )
@@ -43,17 +43,17 @@ bool EpaFace::Initialize()
CollectVertices( m_pVertices );
const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
const btVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const btVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
const SimdScalar e0Sqrd = e0.length2();
const SimdScalar e1Sqrd = e1.length2();
const SimdScalar e0e1 = e0.dot( e1 );
const btScalar e0Sqrd = e0.length2();
const btScalar e1Sqrd = e1.length2();
const btScalar e0e1 = e0.dot( e1 );
m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1;
const SimdScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
const SimdScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );
const btScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
const btScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );
m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0;
m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0;
@@ -83,8 +83,8 @@ bool EpaFace::CalculatePlane()
// Traditional method
const SimdVector3 v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const SimdVector3 v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
const btVector3 v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const btVector3 v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
m_planeNormal = v2.cross( v1 );
@@ -99,10 +99,10 @@ bool EpaFace::CalculatePlane()
// Robust method
//SimdVector3 _v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
//SimdVector3 _v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
//btVector3 _v1 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
//btVector3 _v2 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
//SimdVector3 n;
//btVector3 n;
//n = _v2.cross( _v1 );
@@ -119,8 +119,8 @@ bool EpaFace::CalculatePlane()
//n /= 3;
//n.normalize();
//SimdVector3 c = ( m_pVertices[ 0 ]->m_point + m_pVertices[ 1 ]->m_point + m_pVertices[ 2 ]->m_point ) / 3;
//SimdScalar d = c.dot( -n );
//btVector3 c = ( m_pVertices[ 0 ]->m_point + m_pVertices[ 1 ]->m_point + m_pVertices[ 2 ]->m_point ) / 3;
//btScalar d = c.dot( -n );
//m_robustPlaneNormal = n;
//m_robustPlaneDistance = d;
@@ -142,8 +142,8 @@ bool EpaFace::CalculatePlane()
void EpaFace::CalcClosestPoint()
{
const SimdVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const SimdVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
const btVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
const btVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;
m_v = m_pVertices[ 0 ]->m_point +
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) / m_determinant;
@@ -151,20 +151,20 @@ void EpaFace::CalcClosestPoint()
m_vSqrd = m_v.length2();
}
void EpaFace::CalcClosestPointOnA( SimdVector3& closestPointOnA )
void EpaFace::CalcClosestPointOnA( btVector3& closestPointOnA )
{
const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
const btVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
const btVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnA - m_pVertices[ 0 ]->m_wSupportPointOnA;
closestPointOnA = m_pVertices[ 0 ]->m_wSupportPointOnA +
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) /
m_determinant;
}
void EpaFace::CalcClosestPointOnB( SimdVector3& closestPointOnB )
void EpaFace::CalcClosestPointOnB( btVector3& closestPointOnB )
{
const SimdVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
const SimdVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
const btVector3 e0 = m_pVertices[ 1 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
const btVector3 e1 = m_pVertices[ 2 ]->m_wSupportPointOnB - m_pVertices[ 0 ]->m_wSupportPointOnB;
closestPointOnB = m_pVertices[ 0 ]->m_wSupportPointOnB +
( e0 * m_lambdas[ 0 ] + e1 * m_lambdas[ 1 ] ) /

22
Extras/EPA/EpaFace.h
View File

@@ -21,7 +21,7 @@ class EpaVertex;
class EpaHalfEdge;
#ifdef EPA_POLYHEDRON_USE_PLANES
extern SimdScalar PLANE_THICKNESS;
extern btScalar PLANE_THICKNESS;
#endif
//! Note : This class is not supposed to be a base class
@@ -44,8 +44,8 @@ class EpaFace
bool CalculatePlane();
#endif
void CalcClosestPoint();
void CalcClosestPointOnA( SimdVector3& closestPointOnA );
void CalcClosestPointOnB( SimdVector3& closestPointOnB );
void CalcClosestPointOnA( btVector3& closestPointOnA );
void CalcClosestPointOnB( btVector3& closestPointOnB );
bool IsAffinelyDependent() const;
bool IsClosestPointInternal() const;
@@ -63,18 +63,18 @@ class EpaFace
EpaVertex* m_pVertices[ 3 ];
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdVector3 m_planeNormal;
SimdScalar m_planeDistance;
btVector3 m_planeNormal;
btScalar m_planeDistance;
//SimdVector3 m_robustPlaneNormal;
//SimdScalar m_robustPlaneDistance;
//btVector3 m_robustPlaneNormal;
//btScalar m_robustPlaneDistance;
#endif
SimdVector3 m_v;
SimdScalar m_vSqrd;
btVector3 m_v;
btScalar m_vSqrd;
SimdScalar m_determinant;
SimdScalar m_lambdas[ 2 ];
btScalar m_determinant;
btScalar m_lambdas[ 2 ];
bool m_deleted;
};

74
Extras/EPA/EpaPenetrationDepthSolver.cpp
View File

@@ -14,11 +14,11 @@ subject to the following restrictions:
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 "LinearMath/SimdScalar.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/SimdMinMax.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btPoint3.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btSimdMinMax.h"
#include <list>
@@ -36,19 +36,19 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
#include "NarrowPhaseCollision/EpaPenetrationDepthSolver.h"
SimdScalar g_GJKMaxRelError = 1e-3f;
SimdScalar g_GJKMaxRelErrorSqrd = g_GJKMaxRelError * g_GJKMaxRelError;
btScalar g_GJKMaxRelError = 1e-3f;
btScalar g_GJKMaxRelErrorSqrd = g_GJKMaxRelError * g_GJKMaxRelError;
bool EpaPenetrationDepthSolver::CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
class IDebugDraw* debugDraw )
bool EpaPenetrationDepthSolver::CalcPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
class btIDebugDraw* debugDraw )
{
EPA_DEBUG_ASSERT( pConvexA ,"Convex shape A is invalid!" );
EPA_DEBUG_ASSERT( pConvexB ,"Convex shape B is invalid!" );
SimdScalar penDepth;
btScalar penDepth;
#ifdef EPA_USE_HYBRID
bool needsEPA = !HybridPenDepth( simplexSolver, pConvexA, pConvexB, transformA, transformB,
@@ -70,17 +70,17 @@ bool EpaPenetrationDepthSolver::CalcPenDepth( SimplexSolverInterface& simplexSol
}
#ifdef EPA_USE_HYBRID
bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
SimdScalar& penDepth, SimdVector3& v )
bool EpaPenetrationDepthSolver::HybridPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
btScalar& penDepth, btVector3& v )
{
SimdScalar squaredDistance = SIMD_INFINITY;
SimdScalar delta = 0.f;
btScalar squaredDistance = SIMD_INFINITY;
btScalar delta = 0.f;
const SimdScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin();
const SimdScalar marginSqrd = margin * margin;
const btScalar margin = pConvexA->GetMargin() + pConvexB->GetMargin();
const btScalar marginSqrd = margin * margin;
simplexSolver.reset();
@@ -90,16 +90,16 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
{
assert( ( v.length2() > 0 ) && "Warning: v is the zero vector!" );
SimdVector3 seperatingAxisInA = -v * transformA.getBasis();
SimdVector3 seperatingAxisInB = v * transformB.getBasis();
btVector3 seperatingAxisInA = -v * transformA.getBasis();
btVector3 seperatingAxisInB = v * transformB.getBasis();
SimdVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
SimdVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
btVector3 pInA = pConvexA->LocalGetSupportingVertexWithoutMargin( seperatingAxisInA );
btVector3 qInB = pConvexB->LocalGetSupportingVertexWithoutMargin( seperatingAxisInB );
SimdPoint3 pWorld = transformA( pInA );
SimdPoint3 qWorld = transformB( qInB );
btPoint3 pWorld = transformA( pInA );
btPoint3 qWorld = transformB( qInB );
SimdVector3 w = pWorld - qWorld;
btVector3 w = pWorld - qWorld;
delta = v.dot( w );
// potential exit, they don't overlap
@@ -117,7 +117,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
btScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
@@ -137,7 +137,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
btScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
@@ -148,7 +148,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
return true;
}
SimdScalar previousSquaredDistance = squaredDistance;
btScalar previousSquaredDistance = squaredDistance;
squaredDistance = v.length2();
//are we getting any closer ?
@@ -160,7 +160,7 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
simplexSolver.compute_points( wWitnessOnA, wWitnessOnB );
assert( ( squaredDistance > 0 ) && "squaredDistance is zero!" );
SimdScalar vLength = sqrt( squaredDistance );
btScalar vLength = sqrt( squaredDistance );
wWitnessOnA -= v * ( pConvexA->GetMargin() / vLength );
wWitnessOnB += v * ( pConvexB->GetMargin() / vLength );
@@ -184,10 +184,10 @@ bool EpaPenetrationDepthSolver::HybridPenDepth( SimplexSolverInterface& simplexS
}
#endif
SimdScalar EpaPenetrationDepthSolver::EpaPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB )
btScalar EpaPenetrationDepthSolver::EpaPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btPoint3& wWitnessOnA, btPoint3& wWitnessOnB )
{
Epa epa( pConvexA, pConvexB, transformA, transformB );

34
Extras/EPA/EpaPenetrationDepthSolver.h
View File

@@ -22,34 +22,34 @@ subject to the following restrictions:
* calculate the penetration depth between two convex shapes.
*/
extern SimdScalar g_GJKMaxRelError;
extern SimdScalar g_GJKMaxRelErrorSqrd;
extern btScalar g_GJKMaxRelError;
extern btScalar g_GJKMaxRelErrorSqrd;
//! Note : This class is not supposed to be a base class
class EpaPenetrationDepthSolver : public ConvexPenetrationDepthSolver
class EpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
{
public :
bool CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
class IDebugDraw* debugDraw );
bool CalcPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
class btIDebugDraw* debugDraw );
private :
#ifdef EPA_USE_HYBRID
bool HybridPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB,
SimdScalar& penDepth, SimdVector3& v );
bool HybridPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
btScalar& penDepth, btVector3& v );
#endif
SimdScalar EpaPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* pConvexA, ConvexShape* pConvexB,
const SimdTransform& transformA, const SimdTransform& transformB,
SimdPoint3& wWitnessOnA, SimdPoint3& wWitnessOnB );
btScalar EpaPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btPoint3& wWitnessOnA, btPoint3& wWitnessOnB );
};
#endif // EPA_PENETRATION_DEPTH_H

70
Extras/EPA/EpaPolyhedron.cpp
View File

@@ -14,9 +14,9 @@ subject to the following restrictions:
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 "LinearMath/SimdScalar.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdPoint3.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btPoint3.h"
#include "Memory2.h"
#include <list>
@@ -42,8 +42,8 @@ EpaPolyhedron::~EpaPolyhedron()
Destroy();
}
bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB,
bool EpaPolyhedron::Create( btPoint3* pInitialPoints,
btPoint3* pSupportPointsOnA, btPoint3* pSupportPointsOnB,
const int nbInitialPoints )
{
#ifndef EPA_POLYHEDRON_USE_PLANES
@@ -134,8 +134,8 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{
int axisIndex = axisOrderIndices[ axis ];
SimdScalar axisMin = SIMD_INFINITY;
SimdScalar axisMax = -SIMD_INFINITY;
btScalar axisMin = SIMD_INFINITY;
btScalar axisMax = -SIMD_INFINITY;
for ( int i = 0; i < 4; ++i )
{
@@ -194,36 +194,36 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
//////////////////////////////////////////////////////////////////////////
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
SimdVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
btVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
btVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
#else
SimdVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ];
SimdVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ];
btVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ];
btVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ];
#endif
SimdVector3 planeNormal = v1.cross( v0 );
btVector3 planeNormal = v1.cross( v0 );
planeNormal.normalize();
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal );
btScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal );
#else
SimdScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal );
btScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal );
#endif
#ifdef EPA_POLYHEDRON_USE_PLANES
bool pointOnPlane0 = SimdEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
bool pointOnPlane0 = btEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane0)
{
EPA_DEBUG_ASSERT(0,"Point0 should be on plane!");
return false;
}
bool pointOnPlane1 = SimdEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
bool pointOnPlane1 = btEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane1)
{
EPA_DEBUG_ASSERT(0,"Point1 should be on plane!");
return false;
}
bool pointOnPlane2 = SimdEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
bool pointOnPlane2 = btEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane2)
{
EPA_DEBUG_ASSERT(0,"Point2 should be on plane!");
@@ -235,7 +235,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{
if ( planeDistance > 0 )
{
SimdVector3 tmp = pInitialPoints[ 1 ];
btVector3 tmp = pInitialPoints[ 1 ];
pInitialPoints[ 1 ] = pInitialPoints[ 2 ];
pInitialPoints[ 2 ] = tmp;
@@ -256,16 +256,16 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
#else
finalPointsIndices[ 3 ] = -1;
SimdScalar absMaxDist = -SIMD_INFINITY;
SimdScalar maxDist;
btScalar absMaxDist = -SIMD_INFINITY;
btScalar maxDist;
for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex )
{
SimdScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance;
SimdScalar absDist = abs( dist );
btScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance;
btScalar absDist = abs( dist );
if ( ( absDist > absMaxDist ) &&
!SimdEqual( dist, PLANE_THICKNESS ) )
!btEqual( dist, PLANE_THICKNESS ) )
{
absMaxDist = absDist;
maxDist = dist;
@@ -283,7 +283,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{
// Can swap indices only
SimdPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ];
btPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ];
pInitialPoints[ finalPointsIndices[ 1 ] ] = pInitialPoints[ finalPointsIndices[ 2 ] ];
pInitialPoints[ finalPointsIndices[ 2 ] ] = tmp;
@@ -425,7 +425,7 @@ bool EpaPolyhedron::Create( SimdPoint3* pInitialPoints,
{
EpaFace* pFace = *facesItr;
SimdScalar dist = pFace->m_planeNormal.dot( pInitialPoints[ i ] ) + pFace->m_planeDistance;
btScalar dist = pFace->m_planeNormal.dot( pInitialPoints[ i ] ) + pFace->m_planeDistance;
if ( dist > PLANE_THICKNESS )
{
@@ -486,9 +486,9 @@ EpaHalfEdge* EpaPolyhedron::CreateHalfEdge()
return pNewHalfEdge;
}
EpaVertex* EpaPolyhedron::CreateVertex( const SimdPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB )
EpaVertex* EpaPolyhedron::CreateVertex( const btPoint3& wSupportPoint,
const btPoint3& wSupportPointOnA,
const btPoint3& wSupportPointOnB )
{
EpaVertex* pNewVertex = new EpaVertex( wSupportPoint, wSupportPointOnA, wSupportPointOnB );
EPA_DEBUG_ASSERT( pNewVertex ,"Failed to allocate memory for a new EpaVertex!" );
@@ -542,9 +542,9 @@ void EpaPolyhedron::DestroyAllVertices()
}
}
bool EpaPolyhedron::Expand( const SimdPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB,
bool EpaPolyhedron::Expand( const btPoint3& wSupportPoint,
const btPoint3& wSupportPointOnA,
const btPoint3& wSupportPointOnB,
EpaFace* pFace, std::list< EpaFace* >& newFaces )
{
EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" );
@@ -555,7 +555,7 @@ bool EpaPolyhedron::Expand( const SimdPoint3& wSupportPoint,
// wSupportPoint must be front of face's plane used to do the expansion
#ifdef EPA_POLYHEDRON_USE_PLANES
SimdScalar dist = pFace->m_planeNormal.dot( wSupportPoint ) + pFace->m_planeDistance;
btScalar dist = pFace->m_planeNormal.dot( wSupportPoint ) + pFace->m_planeDistance;
if ( dist <= PLANE_THICKNESS )
{
return false;
@@ -601,7 +601,7 @@ int EpaPolyhedron::GetNbFaces() const
return m_faces.size();
}
void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
void EpaPolyhedron::DeleteVisibleFaces( const btPoint3& point, EpaFace* pFace,
std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges )
{
EPA_DEBUG_ASSERT( !pFace->m_deleted ,"Face is already deleted!" );
@@ -622,12 +622,12 @@ void EpaPolyhedron::DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
#ifdef EPA_POLYHEDRON_USE_PLANES
EPA_DEBUG_ASSERT( ( pAdjacentFace->m_planeNormal.length2() > 0 ) ,"Invalid plane!" );
SimdScalar pointDist = pAdjacentFace->m_planeNormal.dot( point ) +
btScalar pointDist = pAdjacentFace->m_planeNormal.dot( point ) +
pAdjacentFace->m_planeDistance;
if ( pointDist > PLANE_THICKNESS )
#else
SimdScalar dot = pAdjacentFace->m_v.dot( point );
btScalar dot = pAdjacentFace->m_v.dot( point );
if ( dot >= pAdjacentFace->m_vSqrd )
#endif
{

18
Extras/EPA/EpaPolyhedron.h
View File

@@ -34,16 +34,16 @@ class EpaPolyhedron
EpaPolyhedron();
~EpaPolyhedron();
bool Create( SimdPoint3* pInitialPoints,
SimdPoint3* pSupportPointsOnA, SimdPoint3* pSupportPointsOnB,
bool Create( btPoint3* pInitialPoints,
btPoint3* pSupportPointsOnA, btPoint3* pSupportPointsOnB,
const int nbInitialPoints );
void Destroy();
EpaFace* CreateFace();
EpaHalfEdge* CreateHalfEdge();
EpaVertex* CreateVertex( const SimdPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB );
EpaVertex* CreateVertex( const btPoint3& wSupportPoint,
const btPoint3& wSupportPointOnA,
const btPoint3& wSupportPointOnB );
void DeleteFace( EpaFace* pFace );
@@ -51,9 +51,9 @@ class EpaPolyhedron
void DestroyAllHalfEdges();
void DestroyAllVertices();
bool Expand( const SimdPoint3& wSupportPoint,
const SimdPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB,
bool Expand( const btPoint3& wSupportPoint,
const btPoint3& wSupportPointOnA,
const btPoint3& wSupportPointOnB,
EpaFace* pFace, std::list< EpaFace* >& newFaces );
std::list< EpaFace* >& GetFaces();
@@ -61,7 +61,7 @@ class EpaPolyhedron
private :
void DeleteVisibleFaces( const SimdPoint3& point, EpaFace* pFace,
void DeleteVisibleFaces( const btPoint3& point, EpaFace* pFace,
std::list< EpaHalfEdge* >& coneBaseTwinHalfEdges );
void CreateCone( EpaVertex* pAppexVertex, std::list< EpaHalfEdge* >& baseTwinHalfEdges,

14
Extras/EPA/EpaVertex.h
View File

@@ -30,13 +30,13 @@ class EpaVertex
public :
EpaVertex( const SimdPoint3& point ) : /*m_pHalfEdge( 0 ),*/ m_point( point )
EpaVertex( const btPoint3& point ) : /*m_pHalfEdge( 0 ),*/ m_point( point )
{
}
EpaVertex( const SimdPoint3& point,
const SimdPoint3& wSupportPointOnA,
const SimdPoint3& wSupportPointOnB ) : /*m_pHalfEdge( 0 ),*/ m_point( point ),
EpaVertex( const btPoint3& point,
const btPoint3& wSupportPointOnA,
const btPoint3& wSupportPointOnB ) : /*m_pHalfEdge( 0 ),*/ m_point( point ),
m_wSupportPointOnA( wSupportPointOnA ),
m_wSupportPointOnB( wSupportPointOnB )
{
@@ -51,10 +51,10 @@ class EpaVertex
//! This is not necessary
//EpaHalfEdge* m_pHalfEdge;
SimdPoint3 m_point;
btPoint3 m_point;
SimdPoint3 m_wSupportPointOnA;
SimdPoint3 m_wSupportPointOnB;
btPoint3 m_wSupportPointOnA;
btPoint3 m_wSupportPointOnB;
};
#endif

42
Extras/ExtraSolid35/CombinedSimplexSolver.cpp
View File

@@ -30,7 +30,7 @@ void CombinedSimplexSolver::reset()
}
void CombinedSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q)
void CombinedSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
{
printf("addVertex (%f %f %f)\n",w[0],w[1],w[2]);
m_voronoiSolver.addVertex(w,p,q);
@@ -38,8 +38,8 @@ void CombinedSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p,
int i;
i=0;
SimdPoint3 vp1,vp2;
SimdPoint3 jp1,jp2;
btPoint3 vp1,vp2;
btPoint3 jp1,jp2;
/*
bool isClosest0 = m_voronoiSolver.closest(vp1);
bool isClosest1 = m_johnsonSolver.closest(vp1);
@@ -52,12 +52,12 @@ void CombinedSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p,
bool CombinedSimplexSolver::closest(SimdVector3& v)
bool CombinedSimplexSolver::closest(btVector3& v)
{
bool result0 = 0;
bool result1 = 0;
SimdVector3 v0,v1;
btVector3 v0,v1;
result0 = m_voronoiSolver.closest(v0);
result1 = m_johnsonSolver.closest(v1);
@@ -79,10 +79,10 @@ bool CombinedSimplexSolver::closest(SimdVector3& v)
return result1;
}
SimdScalar CombinedSimplexSolver::maxVertex()
btScalar CombinedSimplexSolver::maxVertex()
{
SimdScalar maxv0 = m_voronoiSolver.maxVertex();
SimdScalar maxv1 = m_johnsonSolver.maxVertex();
btScalar maxv0 = m_voronoiSolver.maxVertex();
btScalar maxv1 = m_johnsonSolver.maxVertex();
MY_ASSERT(maxv0 = maxv1);
if (m_useVoronoiSolver)
return maxv0;
@@ -102,7 +102,7 @@ bool CombinedSimplexSolver::fullSimplex() const
return fullSimplex1;
}
int CombinedSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const
int CombinedSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
{
@@ -119,12 +119,12 @@ int CombinedSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVe
void CombinedSimplexSolver::debugPrint()
{
SimdPoint3 pBuf0[4];
SimdPoint3 qBuf0[4];
SimdPoint3 yBuf0[4];
SimdPoint3 pBuf1[4];
SimdPoint3 qBuf1[4];
SimdPoint3 yBuf1[4];
btPoint3 pBuf0[4];
btPoint3 qBuf0[4];
btPoint3 yBuf0[4];
btPoint3 pBuf1[4];
btPoint3 qBuf1[4];
btPoint3 yBuf1[4];
int verts0,verts1;
verts0 = m_voronoiSolver.getSimplex(&pBuf0[0], &qBuf0[0], &yBuf0[0]);
@@ -146,7 +146,7 @@ void CombinedSimplexSolver::debugPrint()
}
bool CombinedSimplexSolver::inSimplex(const SimdVector3& w)
bool CombinedSimplexSolver::inSimplex(const btVector3& w)
{
bool insimplex0 = m_voronoiSolver.inSimplex(w);
bool insimplex1 = m_johnsonSolver.inSimplex(w);
@@ -163,9 +163,9 @@ bool CombinedSimplexSolver::inSimplex(const SimdVector3& w)
return insimplex1;
}
void CombinedSimplexSolver::backup_closest(SimdVector3& v)
void CombinedSimplexSolver::backup_closest(btVector3& v)
{
SimdVector3 v0,v1;
btVector3 v0,v1;
m_voronoiSolver.backup_closest(v0);
m_johnsonSolver.backup_closest(v1);
@@ -189,10 +189,10 @@ bool CombinedSimplexSolver::emptySimplex() const
return empty1;
}
void CombinedSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
void CombinedSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2)
{
SimdPoint3 tmpP1,tmpP2;
SimdPoint3 tmpJP1,tmpJP2;
btPoint3 tmpP1,tmpP2;
btPoint3 tmpJP1,tmpJP2;
m_voronoiSolver.compute_points(tmpP1,tmpP2);
m_johnsonSolver.compute_points(tmpJP1,tmpJP2);

20
Extras/ExtraSolid35/CombinedSimplexSolver.h
View File

@@ -17,10 +17,10 @@
#include "Solid3JohnsonSimplexSolver.h"
/// CombinedSimplexSolver runs both Solid and Voronoi Simplex Solver for comparison
class CombinedSimplexSolver: public SimplexSolverInterface
class CombinedSimplexSolver: public btSimplexSolverInterface
{
VoronoiSimplexSolver m_voronoiSolver;
// VoronoiSimplexSolver m_johnsonSolver;
btVoronoiSimplexSolver m_voronoiSolver;
// btVoronoiSimplexSolver m_johnsonSolver;
Solid3JohnsonSimplexSolver m_johnsonSolver;
@@ -35,23 +35,23 @@ class CombinedSimplexSolver: public SimplexSolverInterface
virtual void reset();
virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q);
virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
virtual bool closest(SimdVector3& v);
virtual bool closest(btVector3& v);
virtual SimdScalar maxVertex();
virtual btScalar maxVertex();
virtual bool fullSimplex() const;
virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const;
virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const;
virtual bool inSimplex(const SimdVector3& w);
virtual bool inSimplex(const btVector3& w);
virtual void backup_closest(SimdVector3& v) ;
virtual void backup_closest(btVector3& v) ;
virtual bool emptySimplex() const;
virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2);
virtual void compute_points(btPoint3& p1, btPoint3& p2);
virtual int numVertices() const;

154
Extras/ExtraSolid35/Solid3EpaPenetrationDepth.cpp
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
@@ -26,34 +26,34 @@
#include <vector>
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "LinearMath/GenMinMax.h"
#include "LinearMath/btMinMax.h"
#define ASSERT_MESSAGE
class ReplaceMeAccuracy {
public:
static SimdScalar rel_error2; // squared relative error in the computed distance
static SimdScalar depth_tolerance; // terminate EPA if upper_bound <= depth_tolerance * dist2
static SimdScalar tol_error; // error tolerance if the distance is almost zero
static btScalar rel_error2; // squared relative error in the computed distance
static btScalar depth_tolerance; // terminate EPA if upper_bound <= depth_tolerance * dist2
static btScalar tol_error; // error tolerance if the distance is almost zero
static void setAccuracy(SimdScalar rel_error)
static void setAccuracy(btScalar rel_error)
{
rel_error2 = rel_error * rel_error;
depth_tolerance = SimdScalar(1.0f) + SimdScalar(2.0f) * rel_error;
depth_tolerance = btScalar(1.0f) + btScalar(2.0f) * rel_error;
}
static void setTolerance(SimdScalar epsilon)
static void setTolerance(btScalar epsilon)
{
tol_error = epsilon;
}
};
static const SimdScalar rel_error = SimdScalar(1.0e-3);
static const btScalar rel_error = btScalar(1.0e-3);
SimdScalar ReplaceMeAccuracy::rel_error2 = rel_error * rel_error;
SimdScalar ReplaceMeAccuracy::depth_tolerance = SimdScalar(1.0) + SimdScalar(2.0) * rel_error;
SimdScalar ReplaceMeAccuracy::tol_error = SIMD_EPSILON;
btScalar ReplaceMeAccuracy::rel_error2 = rel_error * rel_error;
btScalar ReplaceMeAccuracy::depth_tolerance = btScalar(1.0) + btScalar(2.0) * rel_error;
btScalar ReplaceMeAccuracy::tol_error = SIMD_EPSILON;
@@ -101,28 +101,28 @@ public:
bool isObsolete() const { return m_obsolete; }
bool computeClosest(const SimdVector3 *verts);
bool computeClosest(const btVector3 *verts);
const SimdVector3& getClosest() const { return m_closest; }
const btVector3& getClosest() const { return m_closest; }
bool isClosestInternal() const
{
return m_lambda1 >= SimdScalar(0.0) &&
m_lambda2 >= SimdScalar(0.0) &&
return m_lambda1 >= btScalar(0.0) &&
m_lambda2 >= btScalar(0.0) &&
m_lambda1 + m_lambda2 <= m_det;
}
SimdScalar getDist2() const { return m_dist2; }
btScalar getDist2() const { return m_dist2; }
SimdPoint3 getClosestPoint(const SimdPoint3 *points) const
btPoint3 getClosestPoint(const btPoint3 *points) const
{
const SimdPoint3& p0 = points[m_indices[0]];
const btPoint3& p0 = points[m_indices[0]];
return p0 + (m_lambda1 * (points[m_indices[1]] - p0) +
m_lambda2 * (points[m_indices[2]] - p0)) / m_det;
}
void silhouette(const SimdVector3& w, ReplaceMeEdgeBuffer& edgeBuffer)
void silhouette(const btVector3& w, ReplaceMeEdgeBuffer& edgeBuffer)
{
edgeBuffer.clear();
m_obsolete = true;
@@ -132,18 +132,18 @@ public:
}
private:
void silhouette(int index, const SimdVector3& w, ReplaceMeEdgeBuffer& edgeBuffer);
void silhouette(int index, const btVector3& w, ReplaceMeEdgeBuffer& edgeBuffer);
int m_indices[3];
bool m_obsolete;
ReplaceMeFacet *m_adjFacets[3];
int m_adjEdges[3];
SimdScalar m_det;
SimdScalar m_lambda1;
SimdScalar m_lambda2;
SimdVector3 m_closest;
SimdScalar m_dist2;
btScalar m_det;
btScalar m_lambda1;
btScalar m_lambda2;
btVector3 m_closest;
btScalar m_dist2;
};
@@ -163,17 +163,17 @@ bool ReplaceMeFacet::link(int edge0, ReplaceMeFacet *facet, int edge1)
}
bool ReplaceMeFacet::computeClosest(const SimdVector3 *verts)
bool ReplaceMeFacet::computeClosest(const btVector3 *verts)
{
const SimdVector3& p0 = verts[m_indices[0]];
const btVector3& p0 = verts[m_indices[0]];
SimdVector3 v1 = verts[m_indices[1]] - p0;
SimdVector3 v2 = verts[m_indices[2]] - p0;
SimdScalar v1dv1 = v1.length2();
SimdScalar v1dv2 = v1.dot(v2);
SimdScalar v2dv2 = v2.length2();
SimdScalar p0dv1 = p0.dot(v1);
SimdScalar p0dv2 = p0.dot(v2);
btVector3 v1 = verts[m_indices[1]] - p0;
btVector3 v2 = verts[m_indices[2]] - p0;
btScalar v1dv1 = v1.length2();
btScalar v1dv2 = v1.dot(v2);
btScalar v2dv2 = v2.length2();
btScalar p0dv1 = p0.dot(v1);
btScalar p0dv2 = p0.dot(v2);
m_det = v1dv1 * v2dv2 - v1dv2 * v1dv2; // non-negative
//printf("m_det = %f\n",m_det);
@@ -192,7 +192,7 @@ bool ReplaceMeFacet::computeClosest(const SimdVector3 *verts)
return false;
}
void ReplaceMeFacet::silhouette(int index, const SimdVector3& w,
void ReplaceMeFacet::silhouette(int index, const btVector3& w,
ReplaceMeEdgeBuffer& edgeBuffer)
{
if (!m_obsolete) {
@@ -227,9 +227,9 @@ inline int ReplaceMeEdge::getTarget() const
const int MaxSupportPoints = 100;//1000;
const int MaxFacets = 200;//b2000;
static SimdPoint3 pBuf[MaxSupportPoints];
static SimdPoint3 qBuf[MaxSupportPoints];
static SimdVector3 yBuf[MaxSupportPoints];
static btPoint3 pBuf[MaxSupportPoints];
static btPoint3 qBuf[MaxSupportPoints];
static btVector3 yBuf[MaxSupportPoints];
static ReplaceMeFacet facetBuf[MaxFacets];
static int freeFacet = 0;
@@ -249,7 +249,7 @@ public:
ReplaceMeFacetComp myFacetComp;
inline ReplaceMeFacet *addFacet(int i0, int i1, int i2,
SimdScalar lower2, SimdScalar upper2)
btScalar lower2, btScalar upper2)
{
assert(i0 != i1 && i0 != i2 && i1 != i2);
if (freeFacet < MaxFacets)
@@ -300,27 +300,27 @@ inline ReplaceMeFacet *addFacet(int i0, int i1, int i2,
inline bool originInTetrahedron(const SimdVector3& p1, const SimdVector3& p2,
const SimdVector3& p3, const SimdVector3& p4)
inline bool originInTetrahedron(const btVector3& p1, const btVector3& p2,
const btVector3& p3, const btVector3& p4)
{
SimdVector3 normal1 = (p2 - p1).cross(p3 - p1);
SimdVector3 normal2 = (p3 - p2).cross(p4 - p2);
SimdVector3 normal3 = (p4 - p3).cross(p1 - p3);
SimdVector3 normal4 = (p1 - p4).cross(p2 - p4);
btVector3 normal1 = (p2 - p1).cross(p3 - p1);
btVector3 normal2 = (p3 - p2).cross(p4 - p2);
btVector3 normal3 = (p4 - p3).cross(p1 - p3);
btVector3 normal4 = (p1 - p4).cross(p2 - p4);
return
(normal1.dot(p1) > SimdScalar(0.0)) != (normal1.dot(p4) > SimdScalar(0.0)) &&
(normal2.dot(p2) > SimdScalar(0.0)) != (normal2.dot(p1) > SimdScalar(0.0)) &&
(normal3.dot(p3) > SimdScalar(0.0)) != (normal3.dot(p2) > SimdScalar(0.0)) &&
(normal4.dot(p4) > SimdScalar(0.0)) != (normal4.dot(p3) > SimdScalar(0.0));
(normal1.dot(p1) > btScalar(0.0)) != (normal1.dot(p4) > btScalar(0.0)) &&
(normal2.dot(p2) > btScalar(0.0)) != (normal2.dot(p1) > btScalar(0.0)) &&
(normal3.dot(p3) > btScalar(0.0)) != (normal3.dot(p2) > btScalar(0.0)) &&
(normal4.dot(p4) > btScalar(0.0)) != (normal4.dot(p3) > btScalar(0.0));
}
bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb)
bool Solid3EpaPenetrationDepth::CalcPenDepth( btSimplexSolverInterface& simplexSolver,
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transformA,const btTransform& transformB,
btVector3& v, btPoint3& pa, btPoint3& pb)
{
int num_verts = simplexSolver.getSimplex(pBuf, qBuf, yBuf);
@@ -336,17 +336,17 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
// We have a line segment inside the Minkowski sum containing the
// origin. Blow it up by adding three additional support points.
SimdVector3 dir = (yBuf[1] - yBuf[0]).normalized();
btVector3 dir = (yBuf[1] - yBuf[0]).normalized();
int axis = dir.furthestAxis();
static SimdScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;//
static btScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;//
SimdQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, SimdScalar(0.5));
SimdMatrix3x3 rot_mat(rot);
btQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, btScalar(0.5));
btMatrix3x3 rot_mat(rot);
SimdVector3 aux1 = dir.cross(SimdVector3(axis == 0, axis == 1, axis == 2));
SimdVector3 aux2 = rot_mat * aux1;
SimdVector3 aux3 = rot_mat * aux2;
btVector3 aux1 = dir.cross(btVector3(axis == 0, axis == 1, axis == 2));
btVector3 aux2 = rot_mat * aux1;
btVector3 aux3 = rot_mat * aux2;
pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.getBasis()));
qBuf[2] = transformB(convexB->LocalGetSupportingVertex((-aux1)*transformB.getBasis()));
@@ -387,9 +387,9 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
// We have a triangle inside the Minkowski sum containing
// the origin. First blow it up.
SimdVector3 v1 = yBuf[1] - yBuf[0];
SimdVector3 v2 = yBuf[2] - yBuf[0];
SimdVector3 vv = v1.cross(v2);
btVector3 v1 = yBuf[1] - yBuf[0];
btVector3 v2 = yBuf[2] - yBuf[0];
btVector3 vv = v1.cross(v2);
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis()));
@@ -430,15 +430,15 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
num_facets = 0;
freeFacet = 0;
ReplaceMeFacet *f0 = addFacet(0, 1, 2, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f1 = addFacet(0, 3, 1, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f2 = addFacet(0, 2, 3, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f3 = addFacet(1, 3, 2, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f0 = addFacet(0, 1, 2, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f1 = addFacet(0, 3, 1, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f2 = addFacet(0, 2, 3, btScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f3 = addFacet(1, 3, 2, btScalar(0.0), SIMD_INFINITY);
if (!f0 || f0->getDist2() == SimdScalar(0.0) ||
!f1 || f1->getDist2() == SimdScalar(0.0) ||
!f2 || f2->getDist2() == SimdScalar(0.0) ||
!f3 || f3->getDist2() == SimdScalar(0.0))
if (!f0 || f0->getDist2() == btScalar(0.0) ||
!f1 || f1->getDist2() == btScalar(0.0) ||
!f2 || f2->getDist2() == btScalar(0.0) ||
!f3 || f3->getDist2() == btScalar(0.0))
{
return false;
}
@@ -461,7 +461,7 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
ReplaceMeFacet *facet = 0;
SimdScalar upper_bound2 = SIMD_INFINITY;
btScalar upper_bound2 = SIMD_INFINITY;
do {
facet = facetHeap[0];
@@ -470,7 +470,7 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
if (!facet->isObsolete())
{
assert(facet->getDist2() > SimdScalar(0.0));
assert(facet->getDist2() > btScalar(0.0));
if (num_verts == MaxSupportPoints)
{
@@ -487,16 +487,16 @@ bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSol
int index = num_verts++;
SimdScalar far_dist2 = yBuf[index].dot(facet->getClosest());
btScalar far_dist2 = yBuf[index].dot(facet->getClosest());
// Make sure the support mapping is OK.
//assert(far_dist2 > SimdScalar(0.0));
//assert(far_dist2 > btScalar(0.0));
//
// this is to avoid problems with implicit-sphere-touching contact
//
if (far_dist2 < SimdScalar(0.0))
if (far_dist2 < btScalar(0.0))
{
return false;
}

12
Extras/ExtraSolid35/Solid3EpaPenetrationDepth.h
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
@@ -28,14 +28,14 @@
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
/// Solid3EpaPenetrationDepth contains the 'Expanding Polytope Algorithm' from Solid 3.5
class Solid3EpaPenetrationDepth : public ConvexPenetrationDepthSolver
class Solid3EpaPenetrationDepth : public btConvexPenetrationDepthSolver
{
public:
virtual bool CalcPenDepth(SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb);
virtual bool CalcPenDepth(btSimplexSolverInterface& simplexSolver,
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transformA,const btTransform& transformB,
btVector3& v, btPoint3& pa, btPoint3& pb);
};

54
Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.cpp
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
@@ -22,7 +22,7 @@
*/
#include "Solid3JohnsonSimplexSolver.h"
#include "LinearMath/GenMinMax.h"
#include "LinearMath/btMinMax.h"
//#define USE_BACKUP_PROCEDURE
//#define FAST_CLOSEST
@@ -48,7 +48,7 @@ void Solid3JohnsonSimplexSolver::reset()
void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w)
void Solid3JohnsonSimplexSolver::addVertex(const btVector3& w)
{
assert(!fullSimplex());
m_last = 0;
@@ -66,7 +66,7 @@ void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w)
compute_det();
}
void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q)
void Solid3JohnsonSimplexSolver::addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q)
{
addVertex(w);
m_p[m_last] = p;
@@ -83,12 +83,12 @@ bool Solid3JohnsonSimplexSolver::fullSimplex() const
return m_bits1 == 0xf;
}
SimdScalar Solid3JohnsonSimplexSolver::maxVertex()
btScalar Solid3JohnsonSimplexSolver::maxVertex()
{
return m_maxlen2;
}
bool Solid3JohnsonSimplexSolver::closest(SimdVector3& v)
bool Solid3JohnsonSimplexSolver::closest(btVector3& v)
{
#ifdef FAST_CLOSEST
T_Bits s;
@@ -125,14 +125,14 @@ bool Solid3JohnsonSimplexSolver::closest(SimdVector3& v)
// Original GJK calls the backup procedure at this point.
#ifdef USE_BACKUP_PROCEDURE
backup_closest(SimdVector3& v);
backup_closest(btVector3& v);
#endif
return false;
}
int Solid3JohnsonSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const
int Solid3JohnsonSimplexSolver::getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const
{
int num_verts = 0;
int i;
@@ -155,7 +155,7 @@ int Solid3JohnsonSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, S
return num_verts;
}
bool Solid3JohnsonSimplexSolver::inSimplex(const SimdVector3& w)
bool Solid3JohnsonSimplexSolver::inSimplex(const btVector3& w)
{
int i;
T_Bits bit;
@@ -171,18 +171,18 @@ bool Solid3JohnsonSimplexSolver::inSimplex(const SimdVector3& w)
void Solid3JohnsonSimplexSolver::backup_closest(SimdVector3& v)
void Solid3JohnsonSimplexSolver::backup_closest(btVector3& v)
{
SimdScalar min_dist2 = SIMD_INFINITY;
btScalar min_dist2 = SIMD_INFINITY;
T_Bits s;
for (s = m_all_bits; s != 0x0; --s)
{
if (subseteq(s, m_all_bits) && proper(s))
{
SimdVector3 u;
btVector3 u;
compute_vector(s, u);
SimdScalar dist2 = u.length2();
btScalar dist2 = u.length2();
if (dist2 < min_dist2)
{
min_dist2 = dist2;
@@ -195,11 +195,11 @@ void Solid3JohnsonSimplexSolver::backup_closest(SimdVector3& v)
}
void Solid3JohnsonSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
void Solid3JohnsonSimplexSolver::compute_points(btPoint3& p1, btPoint3& p2)
{
SimdScalar sum = SimdScalar(0.0);
p1.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
p2.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
btScalar sum = btScalar(0.0);
p1.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
p2.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
int i;
T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
@@ -212,8 +212,8 @@ void Solid3JohnsonSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
}
}
assert(sum > SimdScalar(0.0));
SimdScalar s = SimdScalar(1.0) / sum;
assert(sum > btScalar(0.0));
btScalar s = btScalar(1.0) / sum;
p1 *= s;
p2 *= s;
}
@@ -272,12 +272,12 @@ bool Solid3JohnsonSimplexSolver::valid(T_Bits s)
{
if (contains(s, bit))
{
if (m_det[s][i] <= SimdScalar(0.0))
if (m_det[s][i] <= btScalar(0.0))
{
return false;
}
}
else if (m_det[s | bit][i] > SimdScalar(0.0))
else if (m_det[s | bit][i] > btScalar(0.0))
{
return false;
}
@@ -292,7 +292,7 @@ bool Solid3JohnsonSimplexSolver::proper(T_Bits s)
T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
{
if (contains(s, bit) && m_det[s][i] <= SimdScalar(0.0))
if (contains(s, bit) && m_det[s][i] <= btScalar(0.0))
{
return false;
}
@@ -300,11 +300,11 @@ bool Solid3JohnsonSimplexSolver::proper(T_Bits s)
return true;
}
void Solid3JohnsonSimplexSolver::compute_vector(T_Bits s, SimdVector3& v)
void Solid3JohnsonSimplexSolver::compute_vector(T_Bits s, btVector3& v)
{
m_maxlen2 = SimdScalar(0.0);
SimdScalar sum = SimdScalar(0.0);
v .setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
m_maxlen2 = btScalar(0.0);
btScalar sum = btScalar(0.0);
v .setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
int i;
T_Bits bit;
@@ -318,7 +318,7 @@ void Solid3JohnsonSimplexSolver::compute_vector(T_Bits s, SimdVector3& v)
}
}
assert(sum > SimdScalar(0.0));
assert(sum > btScalar(0.0));
v /= sum;
}

38
Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.h
View File

@@ -8,7 +8,7 @@
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* GNU bteral Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
@@ -30,7 +30,7 @@
/// Solid3JohnsonSimplexSolver contains Johnson subdistance algorithm from Solid 3.5 library
class Solid3JohnsonSimplexSolver : public SimplexSolverInterface
class Solid3JohnsonSimplexSolver : public btSimplexSolverInterface
{
private:
@@ -42,22 +42,22 @@ private:
void compute_det();
bool valid(T_Bits s);
bool proper(T_Bits s);
void compute_vector(T_Bits s, SimdVector3& v);
void compute_vector(T_Bits s, btVector3& v);
SimdScalar m_det[16][4]; // cached sub-determinants
SimdVector3 m_edge[4][4];
btScalar m_det[16][4]; // cached sub-determinants
btVector3 m_edge[4][4];
#ifdef JOHNSON_ROBUST
SimdScalar m_norm[4][4];
btScalar m_norm[4][4];
#endif
SimdPoint3 m_p[4]; // support points of object A in local coordinates
SimdPoint3 m_q[4]; // support points of object B in local coordinates
SimdVector3 m_y[4]; // support points of A - B in world coordinates
SimdScalar m_ylen2[4]; // Squared lengths support points y
btPoint3 m_p[4]; // support points of object A in local coordinates
btPoint3 m_q[4]; // support points of object B in local coordinates
btVector3 m_y[4]; // support points of A - B in world coordinates
btScalar m_ylen2[4]; // Squared lengths support points y
SimdScalar m_maxlen2; // Maximum squared length to a vertex of the current
btScalar m_maxlen2; // Maximum squared length to a vertex of the current
// simplex
T_Bits m_bits1; // identifies current simplex
T_Bits m_last; // identifies last found support point
@@ -69,7 +69,7 @@ private:
private:
void addVertex(const SimdVector3& w);
void addVertex(const btVector3& w);
public:
Solid3JohnsonSimplexSolver();
@@ -78,23 +78,23 @@ public:
virtual void reset();
virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q);
virtual void addVertex(const btVector3& w, const btPoint3& p, const btPoint3& q);
virtual bool closest(SimdVector3& v);
virtual bool closest(btVector3& v);
virtual SimdScalar maxVertex();
virtual btScalar maxVertex();
virtual bool fullSimplex() const;
virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const;
virtual int getSimplex(btPoint3 *pBuf, btPoint3 *qBuf, btVector3 *yBuf) const;
virtual bool inSimplex(const SimdVector3& w);
virtual bool inSimplex(const btVector3& w);
virtual void backup_closest(SimdVector3& v) ;
virtual void backup_closest(btVector3& v) ;
virtual bool emptySimplex() const ;
virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2) ;
virtual void compute_points(btPoint3& p1, btPoint3& p2) ;
virtual int numVertices() const ;

120
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp
View File

@@ -21,14 +21,14 @@ subject to the following restrictions:
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
#include "BulletCollision/CollisionShapes/btConvexShape.h"
#include "CcdPhysicsEnvironment.h"
#include "LinearMath/SimdTransformUtil.h"
#include "LinearMath/btTransformUtil.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btConeShape.h"
class BP_Proxy;
///todo: fill all the empty CcdPhysicsController methods, hook them up to the RigidBody class
///todo: fill all the empty CcdPhysicsController methods, hook them up to the btRigidBody class
//'temporarily' global variables
float gDeactivationTime = 2.f;
@@ -39,8 +39,8 @@ float gAngularSleepingTreshold = 1.0f;
#include "BulletDynamics/Dynamics/btMassProps.h"
SimdVector3 startVel(0,0,0);//-10000);
CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
btVector3 startVel(0,0,0);//-10000);
CcdPhysicsController::CcdPhysicsController (const btCcdConstructionInfo& ci)
:m_cci(ci)
{
m_collisionDelay = 0;
@@ -61,14 +61,14 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
}
SimdTransform CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
btTransform CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState* motionState)
{
SimdTransform trans;
btTransform trans;
float tmp[3];
motionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
trans.setOrigin(SimdVector3(tmp[0],tmp[1],tmp[2]));
trans.setOrigin(btVector3(tmp[0],tmp[1],tmp[2]));
SimdQuaternion orn;
btQuaternion orn;
motionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
trans.setRotation(orn);
return trans;
@@ -78,11 +78,11 @@ SimdTransform CcdPhysicsController::GetTransformFromMotionState(PHY_IMotionState
void CcdPhysicsController::CreateRigidbody()
{
SimdTransform trans = GetTransformFromMotionState(m_cci.m_MotionState);
btTransform trans = GetTransformFromMotionState(m_cci.m_MotionState);
MassProps mp(m_cci.m_mass, m_cci.m_localInertiaTensor);
btMassProps mp(m_cci.m_mass, m_cci.m_localInertiaTensor);
m_body = new RigidBody(mp,0,0,m_cci.m_friction,m_cci.m_restitution);
m_body = new btRigidBody(mp,0,0,m_cci.m_friction,m_cci.m_restitution);
m_body->m_collisionShape = m_cci.m_collisionShape;
@@ -120,27 +120,27 @@ bool CcdPhysicsController::SynchronizeMotionStates(float time)
if (!m_body->IsStatic())
{
const SimdVector3& worldPos = m_body->getCenterOfMassPosition();
const btVector3& worldPos = m_body->getCenterOfMassPosition();
m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
const SimdQuaternion& worldquat = m_body->getOrientation();
const btQuaternion& worldquat = m_body->getOrientation();
m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
m_MotionState->calculateWorldTransformations();
float scale[3];
m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
SimdVector3 scaling(scale[0],scale[1],scale[2]);
btVector3 scaling(scale[0],scale[1],scale[2]);
GetCollisionShape()->setLocalScaling(scaling);
} else
{
SimdVector3 worldPos;
SimdQuaternion worldquat;
btVector3 worldPos;
btQuaternion worldquat;
m_MotionState->getWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
m_MotionState->getWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
SimdTransform oldTrans = m_body->getCenterOfMassTransform();
SimdTransform newTrans(worldquat,worldPos);
btTransform oldTrans = m_body->getCenterOfMassTransform();
btTransform newTrans(worldquat,worldPos);
m_body->setCenterOfMassTransform(newTrans);
//need to keep track of previous position for friction effects...
@@ -149,7 +149,7 @@ bool CcdPhysicsController::SynchronizeMotionStates(float time)
float scale[3];
m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
SimdVector3 scaling(scale[0],scale[1],scale[2]);
btVector3 scaling(scale[0],scale[1],scale[2]);
GetCollisionShape()->setLocalScaling(scaling);
}
return true;
@@ -219,8 +219,8 @@ void CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dloc
{
m_body->activate();
SimdVector3 dloc(dlocX,dlocY,dlocZ);
SimdTransform xform = m_body->getCenterOfMassTransform();
btVector3 dloc(dlocX,dlocY,dlocZ);
btTransform xform = m_body->getCenterOfMassTransform();
if (local)
{
@@ -239,15 +239,15 @@ void CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
{
m_body->activate();
SimdMatrix3x3 drotmat( rotval[0],rotval[1],rotval[2],
btMatrix3x3 drotmat( rotval[0],rotval[1],rotval[2],
rotval[4],rotval[5],rotval[6],
rotval[8],rotval[9],rotval[10]);
SimdMatrix3x3 currentOrn;
btMatrix3x3 currentOrn;
GetWorldOrientation(currentOrn);
SimdTransform xform = m_body->getCenterOfMassTransform();
btTransform xform = m_body->getCenterOfMassTransform();
xform.setBasis(xform.getBasis()*(local ?
drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
@@ -257,17 +257,17 @@ void CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
}
void CcdPhysicsController::GetWorldOrientation(SimdMatrix3x3& mat)
void CcdPhysicsController::GetWorldOrientation(btMatrix3x3& mat)
{
float orn[4];
m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
SimdQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
btQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
mat.setRotation(quat);
}
void CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
{
SimdQuaternion q = m_body->getCenterOfMassTransform().getRotation();
btQuaternion q = m_body->getCenterOfMassTransform().getRotation();
quatImag0 = q[0];
quatImag1 = q[1];
quatImag2 = q[2];
@@ -277,8 +277,8 @@ void CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float
{
m_body->activate();
m_MotionState->setWorldOrientation(quatImag0,quatImag1,quatImag2,quatReal);
SimdTransform xform = m_body->getCenterOfMassTransform();
xform.setRotation(SimdQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
btTransform xform = m_body->getCenterOfMassTransform();
xform.setRotation(btQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
m_body->setCenterOfMassTransform(xform);
}
@@ -287,8 +287,8 @@ void CcdPhysicsController::setPosition(float posX,float posY,float posZ)
{
m_body->activate();
m_MotionState->setWorldPosition(posX,posY,posZ);
SimdTransform xform = m_body->getCenterOfMassTransform();
xform.setOrigin(SimdVector3(posX,posY,posZ));
btTransform xform = m_body->getCenterOfMassTransform();
xform.setOrigin(btVector3(posX,posY,posZ));
m_body->setCenterOfMassTransform(xform);
}
@@ -298,7 +298,7 @@ void CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvel
void CcdPhysicsController::getPosition(PHY__Vector3& pos) const
{
const SimdTransform& xform = m_body->getCenterOfMassTransform();
const btTransform& xform = m_body->getCenterOfMassTransform();
pos[0] = xform.getOrigin().x();
pos[1] = xform.getOrigin().y();
pos[2] = xform.getOrigin().z();
@@ -306,11 +306,11 @@ void CcdPhysicsController::getPosition(PHY__Vector3& pos) const
void CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
{
if (!SimdFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
!SimdFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
!SimdFuzzyZero(m_cci.m_scaling.z()-scaleZ))
if (!btFuzzyZero(m_cci.m_scaling.x()-scaleX) ||
!btFuzzyZero(m_cci.m_scaling.y()-scaleY) ||
!btFuzzyZero(m_cci.m_scaling.z()-scaleZ))
{
m_cci.m_scaling = SimdVector3(scaleX,scaleY,scaleZ);
m_cci.m_scaling = btVector3(scaleX,scaleY,scaleZ);
if (m_body && m_body->GetCollisionShape())
{
@@ -324,8 +324,8 @@ void CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
// physics methods
void CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
{
SimdVector3 torque(torqueX,torqueY,torqueZ);
SimdTransform xform = m_body->getCenterOfMassTransform();
btVector3 torque(torqueX,torqueY,torqueZ);
btTransform xform = m_body->getCenterOfMassTransform();
if (local)
{
torque = xform.getBasis()*torque;
@@ -335,8 +335,8 @@ void CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torque
void CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
{
SimdVector3 force(forceX,forceY,forceZ);
SimdTransform xform = m_body->getCenterOfMassTransform();
btVector3 force(forceX,forceY,forceZ);
btTransform xform = m_body->getCenterOfMassTransform();
if (local)
{
force = xform.getBasis()*force;
@@ -345,14 +345,14 @@ void CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bo
}
void CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
{
SimdVector3 angvel(ang_velX,ang_velY,ang_velZ);
btVector3 angvel(ang_velX,ang_velY,ang_velZ);
if (angvel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{
m_body->activate();
}
{
SimdTransform xform = m_body->getCenterOfMassTransform();
btTransform xform = m_body->getCenterOfMassTransform();
if (local)
{
angvel = xform.getBasis()*angvel;
@@ -365,14 +365,14 @@ void CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,flo
void CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
{
SimdVector3 linVel(lin_velX,lin_velY,lin_velZ);
btVector3 linVel(lin_velX,lin_velY,lin_velZ);
if (linVel.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{
m_body->activate();
}
{
SimdTransform xform = m_body->getCenterOfMassTransform();
btTransform xform = m_body->getCenterOfMassTransform();
if (local)
{
linVel = xform.getBasis()*linVel;
@@ -382,13 +382,13 @@ void CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,floa
}
void CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
{
SimdVector3 impulse(impulseX,impulseY,impulseZ);
btVector3 impulse(impulseX,impulseY,impulseZ);
if (impulse.length2() > (SIMD_EPSILON*SIMD_EPSILON))
{
m_body->activate();
SimdVector3 pos(attachX,attachY,attachZ);
btVector3 pos(attachX,attachY,attachZ);
m_body->activate();
@@ -402,7 +402,7 @@ void CcdPhysicsController::SetActive(bool active)
// reading out information from physics
void CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
{
const SimdVector3& linvel = this->m_body->getLinearVelocity();
const btVector3& linvel = this->m_body->getLinearVelocity();
linvX = linvel.x();
linvY = linvel.y();
linvZ = linvel.z();
@@ -411,7 +411,7 @@ void CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& l
void CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,float& angVelZ)
{
const SimdVector3& angvel= m_body->getAngularVelocity();
const btVector3& angvel= m_body->getAngularVelocity();
angVelX = angvel.x();
angVelY = angvel.y();
angVelZ = angvel.z();
@@ -419,9 +419,9 @@ void CcdPhysicsController::GetAngularVelocity(float& angVelX,float& angVelY,flo
void CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
{
SimdVector3 pos(posX,posY,posZ);
SimdVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
SimdVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
btVector3 pos(posX,posY,posZ);
btVector3 rel_pos = pos-m_body->getCenterOfMassPosition();
btVector3 linvel = m_body->getVelocityInLocalPoint(rel_pos);
linvX = linvel.x();
linvY = linvel.y();
linvZ = linvel.z();
@@ -436,7 +436,7 @@ void CcdPhysicsController::setRigidBody(bool rigid)
if (!rigid)
{
//fake it for now
SimdVector3 inertia = m_body->getInvInertiaDiagLocal();
btVector3 inertia = m_body->getInvInertiaDiagLocal();
inertia[1] = 0.f;
m_body->setInvInertiaDiagLocal(inertia);
m_body->updateInertiaTensor();
@@ -494,24 +494,24 @@ bool CcdPhysicsController::wantsSleeping()
PHY_IPhysicsController* CcdPhysicsController::GetReplica()
{
//very experimental, shape sharing is not implemented yet.
//just support SphereShape/ConeShape for now
//just support btSphereShape/ConeShape for now
CcdConstructionInfo cinfo = m_cci;
btCcdConstructionInfo cinfo = m_cci;
if (cinfo.m_collisionShape)
{
switch (cinfo.m_collisionShape->GetShapeType())
{
case SPHERE_SHAPE_PROXYTYPE:
{
SphereShape* orgShape = (SphereShape*)cinfo.m_collisionShape;
cinfo.m_collisionShape = new SphereShape(*orgShape);
btSphereShape* orgShape = (btSphereShape*)cinfo.m_collisionShape;
cinfo.m_collisionShape = new btSphereShape(*orgShape);
break;
}
case CONE_SHAPE_PROXYTYPE:
{
ConeShape* orgShape = (ConeShape*)cinfo.m_collisionShape;
cinfo.m_collisionShape = new ConeShape(*orgShape);
btConeShape* orgShape = (btConeShape*)cinfo.m_collisionShape;
cinfo.m_collisionShape = new btConeShape(*orgShape);
break;
}
@@ -570,13 +570,13 @@ void DefaultMotionState::getWorldOrientation(float& quatIma0,float& quatIma1,flo
void DefaultMotionState::setWorldPosition(float posX,float posY,float posZ)
{
SimdPoint3 pos(posX,posY,posZ);
btPoint3 pos(posX,posY,posZ);
m_worldTransform.setOrigin( pos );
}
void DefaultMotionState::setWorldOrientation(float quatIma0,float quatIma1,float quatIma2,float quatReal)
{
SimdQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
btQuaternion orn(quatIma0,quatIma1,quatIma2,quatReal);
m_worldTransform.setRotation( orn );
}

62
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h
View File

@@ -21,16 +21,16 @@ subject to the following restrictions:
/// PHY_IPhysicsController is the abstract simplified Interface to a physical object.
/// It contains the IMotionState and IDeformableMesh Interfaces.
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdScalar.h"
#include "LinearMath/SimdMatrix3x3.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btTransform.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "PHY_IMotionState.h"
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for CollisionShape access
class CollisionShape;
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" //for btCollisionShape access
class btCollisionShape;
extern float gDeactivationTime;
extern float gLinearSleepingTreshold;
@@ -41,12 +41,12 @@ class CcdPhysicsEnvironment;
struct CcdConstructionInfo
struct btCcdConstructionInfo
{
///CollisionFilterGroups provides some optional usage of basic collision filtering
///this is done during broadphase, so very early in the pipeline
///more advanced collision filtering should be done in CollisionDispatcher::NeedsCollision
///more advanced collision filtering should be done in btCollisionDispatcher::NeedsCollision
enum CollisionFilterGroups
{
DefaultFilter = 1,
@@ -57,7 +57,7 @@ struct CcdConstructionInfo
};
CcdConstructionInfo()
btCcdConstructionInfo()
: m_gravity(0,0,0),
m_scaling(1.f,1.f,1.f),
m_mass(0.f),
@@ -74,26 +74,26 @@ struct CcdConstructionInfo
{
}
SimdVector3 m_localInertiaTensor;
SimdVector3 m_gravity;
SimdVector3 m_scaling;
SimdScalar m_mass;
SimdScalar m_restitution;
SimdScalar m_friction;
SimdScalar m_linearDamping;
SimdScalar m_angularDamping;
btVector3 m_localInertiaTensor;
btVector3 m_gravity;
btVector3 m_scaling;
btScalar m_mass;
btScalar m_restitution;
btScalar m_friction;
btScalar m_linearDamping;
btScalar m_angularDamping;
int m_collisionFlags;
///optional use of collision group/mask:
///only collision with object goups that match the collision mask.
///this is very basic early out. advanced collision filtering should be
///done in the CollisionDispatcher::NeedsCollision and NeedsResponse
///done in the btCollisionDispatcher::NeedsCollision and NeedsResponse
///both values default to 1
short int m_collisionFilterGroup;
short int m_collisionFilterMask;
CollisionShape* m_collisionShape;
btCollisionShape* m_collisionShape;
class PHY_IMotionState* m_MotionState;
CcdPhysicsEnvironment* m_physicsEnv; //needed for self-replication
@@ -101,19 +101,19 @@ struct CcdConstructionInfo
};
class RigidBody;
class btRigidBody;
///CcdPhysicsController is a physics object that supports continuous collision detection and time of impact based physics resolution.
class CcdPhysicsController : public PHY_IPhysicsController
{
RigidBody* m_body;
btRigidBody* m_body;
class PHY_IMotionState* m_MotionState;
void* m_newClientInfo;
CcdConstructionInfo m_cci;//needed for replication
void GetWorldOrientation(SimdMatrix3x3& mat);
btCcdConstructionInfo m_cci;//needed for replication
void GetWorldOrientation(btMatrix3x3& mat);
void CreateRigidbody();
@@ -122,14 +122,14 @@ class CcdPhysicsController : public PHY_IPhysicsController
int m_collisionDelay;
CcdPhysicsController (const CcdConstructionInfo& ci);
CcdPhysicsController (const btCcdConstructionInfo& ci);
virtual ~CcdPhysicsController();
RigidBody* GetRigidBody() { return m_body;}
btRigidBody* GetRigidBody() { return m_body;}
CollisionShape* GetCollisionShape() {
btCollisionShape* GetCollisionShape() {
return m_body->GetCollisionShape();
}
////////////////////////////////////
@@ -206,9 +206,9 @@ class CcdPhysicsController : public PHY_IPhysicsController
void UpdateDeactivation(float timeStep);
static SimdTransform GetTransformFromMotionState(PHY_IMotionState* motionState);
static btTransform GetTransformFromMotionState(PHY_IMotionState* motionState);
void SetAabb(const SimdVector3& aabbMin,const SimdVector3& aabbMax);
void SetAabb(const btVector3& aabbMin,const btVector3& aabbMax);
class PHY_IMotionState* GetMotionState()
@@ -227,7 +227,7 @@ class CcdPhysicsController : public PHY_IPhysicsController
///DefaultMotionState implements standard motionstate, using SimdTransform
///DefaultMotionState implements standard motionstate, using btTransform
class DefaultMotionState : public PHY_IMotionState
{
@@ -245,8 +245,8 @@ class DefaultMotionState : public PHY_IMotionState
virtual void calculateWorldTransformations();
SimdTransform m_worldTransform;
SimdVector3 m_localScaling;
btTransform m_worldTransform;
btVector3 m_localScaling;
};

362
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp
View File

File diff suppressed because it is too large Load Diff

66
Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h
View File

@@ -19,16 +19,16 @@ subject to the following restrictions:
#include "PHY_IPhysicsEnvironment.h"
#include <vector>
class CcdPhysicsController;
#include "LinearMath/SimdVector3.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btTransform.h"
class TypedConstraint;
class SimulationIslandManager;
class CollisionDispatcher;
class Dispatcher;
class btTypedConstraint;
class btSimulationIslandManager;
class btCollisionDispatcher;
class btDispatcher;
//#include "btBroadphaseInterface.h"
//switch on/off new vehicle support
@@ -37,10 +37,10 @@ class Dispatcher;
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
class WrapperVehicle;
class PersistentManifold;
class BroadphaseInterface;
class OverlappingPairCache;
class IDebugDraw;
class btPersistentManifold;
class btBroadphaseInterface;
class btOverlappingPairCache;
class btIDebugDraw;
class PHY_IVehicle;
/// CcdPhysicsEnvironment is an experimental mainloop for physics simulation using optional continuous collision detection.
@@ -49,12 +49,12 @@ class PHY_IVehicle;
/// A derived class may be able to 'construct' entities by loading and/or converting
class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment
{
SimdVector3 m_gravity;
btVector3 m_gravity;
protected:
IDebugDraw* m_debugDrawer;
btIDebugDraw* m_debugDrawer;
//solver iterations
int m_numIterations;
@@ -67,12 +67,12 @@ protected:
int m_profileTimings;
bool m_enableSatCollisionDetection;
ContactSolverInfo m_solverInfo;
btContactSolverInfo m_solverInfo;
SimulationIslandManager* m_islandManager;
btSimulationIslandManager* m_islandManager;
public:
CcdPhysicsEnvironment(Dispatcher* dispatcher=0, OverlappingPairCache* pairCache=0);
CcdPhysicsEnvironment(btDispatcher* dispatcher=0, btOverlappingPairCache* pairCache=0);
virtual ~CcdPhysicsEnvironment();
@@ -82,7 +82,7 @@ protected:
/// Perform an integration step of duration 'timeStep'.
virtual void setDebugDrawer(IDebugDraw* debugDrawer)
virtual void setDebugDrawer(btIDebugDraw* debugDrawer)
{
m_debugDrawer = debugDrawer;
}
@@ -127,12 +127,12 @@ protected:
//Following the COLLADA physics specification for constraints
virtual int createUniversalD6Constraint(
class PHY_IPhysicsController* ctrlRef,class PHY_IPhysicsController* ctrlOther,
SimdTransform& localAttachmentFrameRef,
SimdTransform& localAttachmentOther,
const SimdVector3& linearMinLimits,
const SimdVector3& linearMaxLimits,
const SimdVector3& angularMinLimits,
const SimdVector3& angularMaxLimits
btTransform& localAttachmentFrameRef,
btTransform& localAttachmentOther,
const btVector3& linearMinLimits,
const btVector3& linearMaxLimits,
const btVector3& angularMinLimits,
const btVector3& angularMaxLimits
);
@@ -154,7 +154,7 @@ protected:
}
#endif //NEW_BULLET_VEHICLE_SUPPORT
TypedConstraint* getConstraintById(int constraintId);
btTypedConstraint* getConstraintById(int constraintId);
virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ,
float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ);
@@ -183,7 +183,7 @@ protected:
void removeCcdPhysicsController(CcdPhysicsController* ctrl);
BroadphaseInterface* GetBroadphase();
btBroadphaseInterface* GetBroadphase();
@@ -207,34 +207,34 @@ protected:
const PersistentManifold* GetManifold(int index) const;
const btPersistentManifold* GetManifold(int index) const;
std::vector<TypedConstraint*> m_constraints;
std::vector<btTypedConstraint*> m_constraints;
void SyncMotionStates(float timeStep);
class CollisionWorld* GetCollisionWorld()
class btCollisionWorld* GetCollisionWorld()
{
return m_collisionWorld;
}
const class CollisionWorld* GetCollisionWorld() const
const class btCollisionWorld* GetCollisionWorld() const
{
return m_collisionWorld;
}
SimulationIslandManager* GetSimulationIslandManager()
btSimulationIslandManager* GetSimulationIslandManager()
{
return m_islandManager;
}
const SimulationIslandManager* GetSimulationIslandManager() const
const btSimulationIslandManager* GetSimulationIslandManager() const
{
return m_islandManager;
}
class ConstraintSolver* GetConstraintSolver()
class btConstraintSolver* GetConstraintSolver()
{
return m_solver;
}
@@ -253,9 +253,9 @@ protected:
std::vector<WrapperVehicle*> m_wrapperVehicles;
class CollisionWorld* m_collisionWorld;
class btCollisionWorld* m_collisionWorld;
class ConstraintSolver* m_solver;
class btConstraintSolver* m_solver;
bool m_scalingPropagated;

82
Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp
View File

@@ -51,29 +51,29 @@ ParallelIslandDispatcher::ParallelIslandDispatcher ():
};
PersistentManifold* ParallelIslandDispatcher::GetNewManifold(void* b0,void* b1)
btPersistentManifold* ParallelIslandDispatcher::GetNewManifold(void* b0,void* b1)
{
gNumManifold2++;
//ASSERT(gNumManifold < 65535);
CollisionObject* body0 = (CollisionObject*)b0;
CollisionObject* body1 = (CollisionObject*)b1;
btCollisionObject* body0 = (btCollisionObject*)b0;
btCollisionObject* body1 = (btCollisionObject*)b1;
PersistentManifold* manifold = new PersistentManifold (body0,body1);
btPersistentManifold* manifold = new btPersistentManifold (body0,body1);
m_manifoldsPtr.push_back(manifold);
return manifold;
}
void ParallelIslandDispatcher::ClearManifold(PersistentManifold* manifold)
void ParallelIslandDispatcher::ClearManifold(btPersistentManifold* manifold)
{
manifold->ClearManifold();
}
void ParallelIslandDispatcher::ReleaseManifold(PersistentManifold* manifold)
void ParallelIslandDispatcher::ReleaseManifold(btPersistentManifold* manifold)
{
gNumManifold2--;
@@ -82,7 +82,7 @@ void ParallelIslandDispatcher::ReleaseManifold(PersistentManifold* manifold)
ClearManifold(manifold);
std::vector<PersistentManifold*>::iterator i =
std::vector<btPersistentManifold*>::iterator i =
std::find(m_manifoldsPtr.begin(), m_manifoldsPtr.end(), manifold);
if (!(i == m_manifoldsPtr.end()))
{
@@ -99,14 +99,14 @@ void ParallelIslandDispatcher::ReleaseManifold(PersistentManifold* manifold)
//
// todo: this is random access, it can be walked 'cache friendly'!
//
void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback)
void ParallelIslandDispatcher::BuildAndProcessIslands(btCollisionObjectArray& collisionObjects, IslandCallback* callback)
{
int numBodies = collisionObjects.size();
for (int islandId=0;islandId<numBodies;islandId++)
{
std::vector<PersistentManifold*> islandmanifold;
std::vector<btPersistentManifold*> islandmanifold;
//int numSleeping = 0;
@@ -115,7 +115,7 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
btCollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
if (colObj0->GetActivationState()== ACTIVE_TAG)
@@ -132,12 +132,12 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
for (i=0;i<GetNumManifolds();i++)
{
PersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
btPersistentManifold* manifold = this->GetManifoldByIndexInternal(i);
//filtering for response
CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->GetBody0());
btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->GetBody1());
{
if (((colObj0) && (colObj0)->m_islandTag1 == (islandId)) ||
((colObj1) && (colObj1)->m_islandTag1 == (islandId)))
@@ -153,7 +153,7 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
btCollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
colObj0->SetActivationState( ISLAND_SLEEPING );
@@ -167,7 +167,7 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
int i;
for (i=0;i<numBodies;i++)
{
CollisionObject* colObj0 = collisionObjects[i];
btCollisionObject* colObj0 = collisionObjects[i];
if (colObj0->m_islandTag1 == islandId)
{
if ( colObj0->GetActivationState() == ISLAND_SLEEPING)
@@ -189,73 +189,73 @@ void ParallelIslandDispatcher::BuildAndProcessIslands(CollisionObjectArray& coll
CollisionAlgorithm* ParallelIslandDispatcher::InternalFindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
btCollisionAlgorithm* ParallelIslandDispatcher::InternalFindAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
{
m_count++;
CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject;
CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject;
btCollisionObject* body0 = (btCollisionObject*)proxy0.m_clientObject;
btCollisionObject* body1 = (btCollisionObject*)proxy1.m_clientObject;
CollisionAlgorithmConstructionInfo ci;
btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher = this;
if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConvex() )
{
return new ConvexConvexAlgorithm(0,ci,&proxy0,&proxy1);
return new btConvexConvexAlgorithm(0,ci,&proxy0,&proxy1);
}
if (body0->m_collisionShape->IsConvex() && body1->m_collisionShape->IsConcave())
{
return new ConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1);
return new btConvexConcaveCollisionAlgorithm(ci,&proxy0,&proxy1);
}
if (body1->m_collisionShape->IsConvex() && body0->m_collisionShape->IsConcave())
{
return new ConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0);
return new btConvexConcaveCollisionAlgorithm(ci,&proxy1,&proxy0);
}
if (body0->m_collisionShape->IsCompound())
{
return new CompoundCollisionAlgorithm(ci,&proxy0,&proxy1);
return new btCompoundCollisionAlgorithm(ci,&proxy0,&proxy1);
} else
{
if (body1->m_collisionShape->IsCompound())
{
return new CompoundCollisionAlgorithm(ci,&proxy1,&proxy0);
return new btCompoundCollisionAlgorithm(ci,&proxy1,&proxy0);
}
}
//failed to find an algorithm
return new EmptyAlgorithm(ci);
return new btEmptyAlgorithm(ci);
}
bool ParallelIslandDispatcher::NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1)
bool ParallelIslandDispatcher::NeedsResponse(const btCollisionObject& colObj0,const btCollisionObject& colObj1)
{
//here you can do filtering
bool hasResponse =
(!(colObj0.m_collisionFlags & CollisionObject::noContactResponse)) &&
(!(colObj1.m_collisionFlags & CollisionObject::noContactResponse));
(!(colObj0.m_collisionFlags & btCollisionObject::noContactResponse)) &&
(!(colObj1.m_collisionFlags & btCollisionObject::noContactResponse));
hasResponse = hasResponse &&
(colObj0.IsActive() || colObj1.IsActive());
return hasResponse;
}
bool ParallelIslandDispatcher::NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
bool ParallelIslandDispatcher::NeedsCollision(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
{
CollisionObject* body0 = (CollisionObject*)proxy0.m_clientObject;
CollisionObject* body1 = (CollisionObject*)proxy1.m_clientObject;
btCollisionObject* body0 = (btCollisionObject*)proxy0.m_clientObject;
btCollisionObject* body1 = (btCollisionObject*)proxy1.m_clientObject;
assert(body0);
assert(body1);
bool needsCollision = true;
if ((body0->m_collisionFlags & CollisionObject::isStatic) &&
(body1->m_collisionFlags & CollisionObject::isStatic))
if ((body0->m_collisionFlags & btCollisionObject::isStatic) &&
(body1->m_collisionFlags & btCollisionObject::isStatic))
needsCollision = false;
if ((!body0->IsActive()) && (!body1->IsActive()))
@@ -266,23 +266,23 @@ bool ParallelIslandDispatcher::NeedsCollision(BroadphaseProxy& proxy0,Broadphase
}
///allows the user to get contact point callbacks
ManifoldResult* ParallelIslandDispatcher::GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold)
btManifoldResult* ParallelIslandDispatcher::GetNewManifoldResult(btCollisionObject* obj0,btCollisionObject* obj1,btPersistentManifold* manifold)
{
//in-place, this prevents parallel dispatching, but just adding a list would fix that.
ManifoldResult* manifoldResult = new (&m_defaultManifoldResult) ManifoldResult(obj0,obj1,manifold);
btManifoldResult* manifoldResult = new (&m_defaultManifoldResult) btManifoldResult(obj0,obj1,manifold);
return manifoldResult;
}
///allows the user to get contact point callbacks
void ParallelIslandDispatcher::ReleaseManifoldResult(ManifoldResult*)
void ParallelIslandDispatcher::ReleaseManifoldResult(btManifoldResult*)
{
}
void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo)
void ParallelIslandDispatcher::DispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)
{
//m_blockedForChanges = true;
@@ -296,7 +296,7 @@ void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* p
for (i=0;i<numPairs;i++)
{
BroadphasePair& pair = pairs[i];
btBroadphasePair& pair = pairs[i];
if (dispatcherId>= 0)
{
@@ -310,7 +310,7 @@ void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* p
if (pair.m_algorithms[dispatcherId])
{
if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE)
if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE)
{
pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
} else
@@ -324,13 +324,13 @@ void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* p
} else
{
//non-persistent algorithm dispatcher
CollisionAlgorithm* algo = FindAlgorithm(
btCollisionAlgorithm* algo = FindAlgorithm(
*pair.m_pProxy0,
*pair.m_pProxy1);
if (algo)
{
if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE)
if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE)
{
algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo);
} else

44
Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h
View File

@@ -24,7 +24,7 @@ subject to the following restrictions:
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include <vector>
class IDebugDraw;
class btIDebugDraw;
#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
@@ -35,28 +35,28 @@ class IDebugDraw;
///ParallelIslandDispatcher dispatches entire simulation islands in parallel.
///For both collision detection and constraint solving.
///This development heads toward multi-core, CELL SPU and GPU approach
class ParallelIslandDispatcher : public Dispatcher
class ParallelIslandDispatcher : public btDispatcher
{
std::vector<PersistentManifold*> m_manifoldsPtr;
std::vector<btPersistentManifold*> m_manifoldsPtr;
UnionFind m_unionFind;
btUnionFind m_unionFind;
bool m_useIslands;
ManifoldResult m_defaultManifoldResult;
btManifoldResult m_defaultManifoldResult;
CollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
public:
UnionFind& GetUnionFind() { return m_unionFind;}
btUnionFind& GetUnionFind() { return m_unionFind;}
struct IslandCallback
{
virtual ~IslandCallback() {};
virtual void ProcessIsland(PersistentManifold** manifolds,int numManifolds) = 0;
virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds) = 0;
};
@@ -65,12 +65,12 @@ public:
return m_manifoldsPtr.size();
}
PersistentManifold* GetManifoldByIndexInternal(int index)
btPersistentManifold* GetManifoldByIndexInternal(int index)
{
return m_manifoldsPtr[index];
}
const PersistentManifold* GetManifoldByIndexInternal(int index) const
const btPersistentManifold* GetManifoldByIndexInternal(int index) const
{
return m_manifoldsPtr[index];
}
@@ -88,37 +88,37 @@ public:
ParallelIslandDispatcher ();
virtual ~ParallelIslandDispatcher() {};
virtual PersistentManifold* GetNewManifold(void* b0,void* b1);
virtual btPersistentManifold* GetNewManifold(void* b0,void* b1);
virtual void ReleaseManifold(PersistentManifold* manifold);
virtual void ReleaseManifold(btPersistentManifold* manifold);
virtual void BuildAndProcessIslands(CollisionObjectArray& collisionObjects, IslandCallback* callback);
virtual void BuildAndProcessIslands(btCollisionObjectArray& collisionObjects, IslandCallback* callback);
///allows the user to get contact point callbacks
virtual ManifoldResult* GetNewManifoldResult(CollisionObject* obj0,CollisionObject* obj1,PersistentManifold* manifold);
virtual btManifoldResult* GetNewManifoldResult(btCollisionObject* obj0,btCollisionObject* obj1,btPersistentManifold* manifold);
///allows the user to get contact point callbacks
virtual void ReleaseManifoldResult(ManifoldResult*);
virtual void ReleaseManifoldResult(btManifoldResult*);
virtual void ClearManifold(PersistentManifold* manifold);
virtual void ClearManifold(btPersistentManifold* manifold);
CollisionAlgorithm* FindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1)
btCollisionAlgorithm* FindAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1)
{
CollisionAlgorithm* algo = InternalFindAlgorithm(proxy0,proxy1);
btCollisionAlgorithm* algo = InternalFindAlgorithm(proxy0,proxy1);
return algo;
}
CollisionAlgorithm* InternalFindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1);
btCollisionAlgorithm* InternalFindAlgorithm(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1);
virtual bool NeedsCollision(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1);
virtual bool NeedsCollision(btBroadphaseProxy& proxy0,btBroadphaseProxy& proxy1);
virtual bool NeedsResponse(const CollisionObject& colObj0,const CollisionObject& colObj1);
virtual bool NeedsResponse(const btCollisionObject& colObj0,const btCollisionObject& colObj1);
virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;}
virtual void DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo);
virtual void DispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo);

40
Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp
View File

@@ -24,7 +24,7 @@ subject to the following restrictions:
#include "SimulationIsland.h"
ParallelPhysicsEnvironment::ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher, OverlappingPairCache* pairCache):
ParallelPhysicsEnvironment::ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher, btOverlappingPairCache* pairCache):
CcdPhysicsEnvironment(dispatcher,pairCache)
{
@@ -41,14 +41,14 @@ ParallelPhysicsEnvironment::~ParallelPhysicsEnvironment()
bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
{
// Make sure the broadphase / overlapping AABB paircache is up-to-date
OverlappingPairCache* scene = m_collisionWorld->GetPairCache();
btOverlappingPairCache* scene = m_collisionWorld->GetPairCache();
scene->RefreshOverlappingPairs();
// Find the connected sets that can be simulated in parallel
// Using union find
#ifdef USE_QUICKPROF
Profiler::beginBlock("IslandUnionFind");
btProfiler::beginBlock("IslandUnionFind");
#endif //USE_QUICKPROF
GetSimulationIslandManager()->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher());
@@ -58,10 +58,10 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
int numConstraints = m_constraints.size();
for (i=0;i< numConstraints ; i++ )
{
TypedConstraint* constraint = m_constraints[i];
btTypedConstraint* constraint = m_constraints[i];
const RigidBody* colObj0 = &constraint->GetRigidBodyA();
const RigidBody* colObj1 = &constraint->GetRigidBodyB();
const btRigidBody* colObj0 = &constraint->GetRigidBodyA();
const btRigidBody* colObj1 = &constraint->GetRigidBodyB();
if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
((colObj1) && ((colObj1)->mergesSimulationIslands())))
@@ -80,7 +80,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
GetSimulationIslandManager()->StoreIslandActivationState(GetCollisionWorld());
#ifdef USE_QUICKPROF
Profiler::endBlock("IslandUnionFind");
btProfiler::endBlock("IslandUnionFind");
#endif //USE_QUICKPROF
@@ -88,7 +88,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
///build simulation islands
#ifdef USE_QUICKPROF
Profiler::beginBlock("BuildIslands");
btProfiler::beginBlock("BuildIslands");
#endif //USE_QUICKPROF
std::vector<SimulationIsland> simulationIslands;
@@ -105,7 +105,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
}
}
Dispatcher* dispatcher = GetCollisionWorld()->GetDispatcher();
btDispatcher* dispatcher = GetCollisionWorld()->GetDispatcher();
//this is a brute force approach, will rethink later about more subtle ways
@@ -117,10 +117,10 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
{
BroadphasePair* pair = &scene->GetOverlappingPair(i);
btBroadphasePair* pair = &scene->GetOverlappingPair(i);
CollisionObject* col0 = static_cast<CollisionObject*>(pair->m_pProxy0->m_clientObject);
CollisionObject* col1 = static_cast<CollisionObject*>(pair->m_pProxy1->m_clientObject);
btCollisionObject* col0 = static_cast<btCollisionObject*>(pair->m_pProxy0->m_clientObject);
btCollisionObject* col1 = static_cast<btCollisionObject*>(pair->m_pProxy1->m_clientObject);
if (col0->m_islandTag1 > col1->m_islandTag1)
{
@@ -136,7 +136,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
//store constraint indices for each island
for (unsigned int ui=0;ui<m_constraints.size();ui++)
{
TypedConstraint& constraint = *m_constraints[ui];
btTypedConstraint& constraint = *m_constraints[ui];
if (constraint.GetRigidBodyA().m_islandTag1 > constraint.GetRigidBodyB().m_islandTag1)
{
simulationIslands[constraint.GetRigidBodyA().m_islandTag1].m_constraintIndices.push_back(ui);
@@ -151,12 +151,12 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
for (i=0;i<dispatcher->GetNumManifolds();i++)
{
PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
btPersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i);
//filtering for response
CollisionObject* colObj0 = static_cast<CollisionObject*>(manifold->GetBody0());
CollisionObject* colObj1 = static_cast<CollisionObject*>(manifold->GetBody1());
btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->GetBody0());
btCollisionObject* colObj1 = static_cast<btCollisionObject*>(manifold->GetBody1());
{
int islandTag = colObj0->m_islandTag1;
if (colObj1->m_islandTag1 > islandTag)
@@ -169,15 +169,15 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
}
#ifdef USE_QUICKPROF
Profiler::endBlock("BuildIslands");
btProfiler::endBlock("BuildIslands");
#endif //USE_QUICKPROF
#ifdef USE_QUICKPROF
Profiler::beginBlock("SimulateIsland");
btProfiler::beginBlock("SimulateIsland");
#endif //USE_QUICKPROF
TypedConstraint** constraintBase = 0;
btTypedConstraint** constraintBase = 0;
if (m_constraints.size())
constraintBase = &m_constraints[0];
@@ -198,7 +198,7 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep)
#ifdef USE_QUICKPROF
Profiler::endBlock("SimulateIsland");
btProfiler::endBlock("SimulateIsland");
#endif //USE_QUICKPROF
return true;

2
Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.h
View File

@@ -29,7 +29,7 @@ class ParallelPhysicsEnvironment : public CcdPhysicsEnvironment
public:
ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher=0, OverlappingPairCache* pairCache=0);
ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher=0, btOverlappingPairCache* pairCache=0);
virtual ~ParallelPhysicsEnvironment();

74
Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp
View File

@@ -14,7 +14,7 @@ subject to the following restrictions:
*/
#include "SimulationIsland.h"
#include "LinearMath/SimdTransform.h"
#include "LinearMath/btTransform.h"
#include "CcdPhysicsController.h"
#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
@@ -22,17 +22,17 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btIDebugDraw.h"
extern float gContactBreakingTreshold;
bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,TypedConstraint** constraintsBaseAddress,BroadphasePair* overlappingPairBaseAddress, Dispatcher* dispatcher,BroadphaseInterface* broadphase,class ConstraintSolver* solver,float timeStep)
bool SimulationIsland::Simulate(btIDebugDraw* debugDrawer,int numSolverIterations,btTypedConstraint** constraintsBaseAddress,btBroadphasePair* overlappingPairBaseAddress, btDispatcher* dispatcher,btBroadphaseInterface* broadphase,class btConstraintSolver* solver,float timeStep)
{
#ifdef USE_QUICKPROF
Profiler::beginBlock("predictIntegratedTransform");
btProfiler::beginBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF
{
@@ -44,8 +44,8 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
for (k=0;k<GetNumControllers();k++)
{
CcdPhysicsController* ctrl = m_controllers[k];
// SimdTransform predictedTrans;
RigidBody* body = ctrl->GetRigidBody();
// btTransform predictedTrans;
btRigidBody* body = ctrl->GetRigidBody();
//todo: only do this when necessary, it's used for contact points
body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse();
@@ -63,7 +63,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
}
#ifdef USE_QUICKPROF
Profiler::endBlock("predictIntegratedTransform");
btProfiler::endBlock("predictIntegratedTransform");
#endif //USE_QUICKPROF
//BroadphaseInterface* scene = GetBroadphase();
@@ -75,20 +75,20 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
#ifdef USE_QUICKPROF
Profiler::beginBlock("DispatchAllCollisionPairs");
btProfiler::beginBlock("DispatchAllCollisionPairs");
#endif //USE_QUICKPROF
// int numsubstep = m_numIterations;
DispatcherInfo dispatchInfo;
btDispatcherInfo dispatchInfo;
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_enableSatConvex = false;//m_enableSatCollisionDetection;
dispatchInfo.m_debugDraw = debugDrawer;
std::vector<BroadphasePair> overlappingPairs;
std::vector<btBroadphasePair> overlappingPairs;
overlappingPairs.resize(this->m_overlappingPairIndices.size());
//gather overlapping pair info
@@ -115,13 +115,13 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
#ifdef USE_QUICKPROF
Profiler::endBlock("DispatchAllCollisionPairs");
btProfiler::endBlock("DispatchAllCollisionPairs");
#endif //USE_QUICKPROF
//contacts
#ifdef USE_QUICKPROF
Profiler::beginBlock("SolveConstraint");
btProfiler::beginBlock("SolveConstraint");
#endif //USE_QUICKPROF
@@ -139,7 +139,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
//point to point constraints
for (i=0;i< numConstraints ; i++ )
{
TypedConstraint* constraint = constraintsBaseAddress[m_constraintIndices[i]];
btTypedConstraint* constraint = constraintsBaseAddress[m_constraintIndices[i]];
constraint->BuildJacobian();
constraint->SolveConstraint( timeStep );
@@ -149,7 +149,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
}
#ifdef USE_QUICKPROF
Profiler::endBlock("SolveConstraint");
btProfiler::endBlock("SolveConstraint");
#endif //USE_QUICKPROF
/*
@@ -162,7 +162,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
for (int i=0;i<numVehicles;i++)
{
WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i];
RaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
btRaycastVehicle* vehicle = wrapperVehicle->GetVehicle();
vehicle->UpdateVehicle( timeStep);
}
#endif //NEW_BULLET_VEHICLE_SUPPORT
@@ -170,20 +170,20 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
/*
Profiler::beginBlock("CallbackTriggers");
btProfiler::beginBlock("CallbackTriggers");
#endif //USE_QUICKPROF
CallbackTriggers();
#ifdef USE_QUICKPROF
Profiler::endBlock("CallbackTriggers");
btProfiler::endBlock("CallbackTriggers");
}
*/
//OverlappingPairCache* scene = GetCollisionWorld()->GetPairCache();
ContactSolverInfo solverInfo;
btContactSolverInfo solverInfo;
solverInfo.m_friction = 0.9f;
solverInfo.m_numIterations = numSolverIterations;
@@ -198,7 +198,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
#ifdef USE_QUICKPROF
Profiler::beginBlock("proceedToTransform");
btProfiler::beginBlock("proceedToTransform");
#endif //USE_QUICKPROF
{
@@ -216,10 +216,10 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
/* if (m_ccdMode == 3)
{
DispatcherInfo dispatchInfo;
btDispatcherInfo dispatchInfo;
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS;
dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS;
// GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo);
toi = dispatchInfo.m_timeOfImpact;
@@ -243,8 +243,8 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
CcdPhysicsController* ctrl = *i;
SimdTransform predictedTrans;
RigidBody* body = ctrl->GetRigidBody();
btTransform predictedTrans;
btRigidBody* body = ctrl->GetRigidBody();
if (body->IsActive())
{
@@ -276,7 +276,7 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
!(i==m_controllers.end()); i++)
{
CcdPhysicsController* ctrl = (*i);
RigidBody* body = ctrl->GetRigidBody();
btRigidBody* body = ctrl->GetRigidBody();
ctrl->UpdateDeactivation(timeStep);
@@ -313,15 +313,15 @@ bool SimulationIsland::Simulate(IDebugDraw* debugDrawer,int numSolverIterations,
#ifdef USE_QUICKPROF
Profiler::endBlock("proceedToTransform");
btProfiler::endBlock("proceedToTransform");
Profiler::beginBlock("SyncMotionStates");
btProfiler::beginBlock("SyncMotionStates");
#endif //USE_QUICKPROF
SyncMotionStates(timeStep);
#ifdef USE_QUICKPROF
Profiler::endBlock("SyncMotionStates");
btProfiler::endBlock("SyncMotionStates");
#endif //USE_QUICKPROF
@@ -363,7 +363,7 @@ void SimulationIsland::SyncMotionStates(float timeStep)
void SimulationIsland::UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface* scene,float timeStep)
void SimulationIsland::UpdateAabbs(btIDebugDraw* debugDrawer,btBroadphaseInterface* scene,float timeStep)
{
std::vector<CcdPhysicsController*>::iterator i;
@@ -375,33 +375,33 @@ void SimulationIsland::UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface*
!(i==m_controllers.end()); i++)
{
CcdPhysicsController* ctrl = (*i);
RigidBody* body = ctrl->GetRigidBody();
btRigidBody* body = ctrl->GetRigidBody();
SimdPoint3 minAabb,maxAabb;
CollisionShape* shapeinterface = ctrl->GetCollisionShape();
btPoint3 minAabb,maxAabb;
btCollisionShape* shapeinterface = ctrl->GetCollisionShape();
shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(),
body->getLinearVelocity(),
//body->getAngularVelocity(),
SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(),
btVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(),
timeStep,minAabb,maxAabb);
SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
btVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold);
minAabb -= manifoldExtraExtents;
maxAabb += manifoldExtraExtents;
BroadphaseProxy* bp = body->m_broadphaseHandle;
btBroadphaseProxy* bp = body->m_broadphaseHandle;
if (bp)
{
SimdVector3 color (1,1,0);
btVector3 color (1,1,0);
/*
class IDebugDraw* m_debugDrawer = 0;
class btIDebugDraw* m_debugDrawer = 0;
if (m_debugDrawer)
{
//draw aabb
@@ -428,7 +428,7 @@ void SimulationIsland::UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface*
};
if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb)
if (m_debugDrawer->GetDebugMode() & btIDebugDraw::DBG_DrawAabb)
{
DrawAabb(m_debugDrawer,minAabb,maxAabb,color);
}

12
Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h
View File

@@ -17,9 +17,9 @@ subject to the following restrictions:
#define SIMULATION_ISLAND_H
#include <vector>
class BroadphaseInterface;
class Dispatcher;
class IDebugDraw;
class btBroadphaseInterface;
class btDispatcher;
class btIDebugDraw;
///SimulationIsland groups all computations and data (for collision detection and dynamics) that can execute in parallel with other SimulationIsland's
///The ParallelPhysicsEnvironment and ParallelIslandDispatcher will dispatch SimulationIsland's
@@ -30,12 +30,12 @@ class SimulationIsland
public:
std::vector<class CcdPhysicsController*> m_controllers;
std::vector<class PersistentManifold*> m_manifolds;
std::vector<class btPersistentManifold*> m_manifolds;
std::vector<int> m_overlappingPairIndices;
std::vector<int> m_constraintIndices;
bool Simulate(IDebugDraw* debugDrawer,int numSolverIterations,class TypedConstraint** constraintsBaseAddress,struct BroadphasePair* overlappingPairBaseAddress, Dispatcher* dispatcher,BroadphaseInterface* broadphase, class ConstraintSolver* solver, float timeStep);
bool Simulate(btIDebugDraw* debugDrawer,int numSolverIterations,class btTypedConstraint** constraintsBaseAddress,struct btBroadphasePair* overlappingPairBaseAddress, btDispatcher* dispatcher,btBroadphaseInterface* broadphase, class btConstraintSolver* solver, float timeStep);
int GetNumControllers()
@@ -47,7 +47,7 @@ class SimulationIsland
void SyncMotionStates(float timeStep);
void UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface* broadphase,float timeStep);
void UpdateAabbs(btIDebugDraw* debugDrawer,btBroadphaseInterface* broadphase,float timeStep);
};
#endif //SIMULATION_ISLAND_H

4
Extras/PhysicsInterface/Common/PHY_Pro.h
View File

@@ -17,7 +17,7 @@ subject to the following restrictions:
#define PHY_PROPSH
class CollisionShape;
class btCollisionShape;
// Properties of dynamic objects
struct PHY_ShapeProps {
@@ -29,7 +29,7 @@ struct PHY_ShapeProps {
bool m_do_anisotropic; // Should I do anisotropic friction?
bool m_do_fh; // Should the object have a linear Fh spring?
bool m_do_rot_fh; // Should the object have an angular Fh spring?
CollisionShape* m_shape;
btCollisionShape* m_shape;
};

22
Extras/SATConvexCollision/Hull.cpp
View File

@@ -540,7 +540,7 @@ Hull* Hull::MakeHullFromTemp()
void Hull::ProcessHullHull(Separation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB,HullContactCollector* collector)
void Hull::ProcessHullHull(btSeparation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB,HullContactCollector* collector)
{
Point3 vertsA[Hull::kMaxVerts];
Point3 vertsB[Hull::kMaxVerts];
@@ -563,7 +563,7 @@ void Hull::ProcessHullHull(Separation& sep,const Hull& shapeA,const Hull& shapeB
#ifdef SHAPE_COLLIDER_USE_CACHING
// update cached pair
if (sep.m_separator != Separation::kFeatureNone)
if (sep.m_separator != btSeparation::kFeatureNone)
{
// re-use the separation
if (UpdateSeparationHullHull(sep, pVertsA, pVertsB, trA, trB) == true)
@@ -756,7 +756,7 @@ int Hull::ClipFace(int numVerts, Point3** ppVtxIn, Point3** ppVtxOut, const Plan
int Hull::AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB,
int Hull::AddContactsHullHull(btSeparation& sep, const Point3* pVertsA, const Point3* pVertsB,
const Transform& trA, const Transform& trB,const Hull& hullA,const Hull& hullB,
HullContactCollector* hullContactCollector)
{
@@ -765,7 +765,7 @@ int Hull::AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Poin
Vector3 normalWorld = sep.m_axis;
// edge->edge contact is always a single point
if (sep.m_separator == Separation::kFeatureBoth)
if (sep.m_separator == btSeparation::kFeatureBoth)
{
const Hull::Edge& edgeA = hullA.GetEdge(sep.m_featureA);
const Hull::Edge& edgeB = hullB.GetEdge(sep.m_featureB);
@@ -799,7 +799,7 @@ int Hull::AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Poin
// find face of hull A that is most opposite contact axis
// TODO: avoid having to transform planes here
if (sep.m_separator == Separation::kFeatureB)
if (sep.m_separator == btSeparation::kFeatureB)
{
const Hull::Edge& edgeB = hullB.GetEdge(hullB.GetFaceFirstEdge(faceB));
tangent = Normalize(pVertsB[edgeB.m_verts[1]] - pVertsB[edgeB.m_verts[0]]);
@@ -921,7 +921,7 @@ int Hull::AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Poin
// if no separating axis was found then details of least penetrating axis are returned
// resulting axis always points away from hullB
// either transform can be null in which case it is treated as identity (this avoids a bunch of work)
bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB,
bool Hull::GetSeparationHullHull(btSeparation& sep, const Point3* pVertsA, const Point3* pVertsB,
const Transform& trA, const Transform& trB,
const Hull& hullA,
const Hull& hullB
@@ -930,7 +930,7 @@ bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const P
//const Hull& hullA((const Hull&)*sep.m_pShapeA->GetShape());
//const Hull& hullB((const Hull&)*sep.m_pShapeB->GetShape());
sep.m_separator = Separation::kFeatureNone;
sep.m_separator = btSeparation::kFeatureNone;
sep.m_dist = MinValueF;
sep.m_featureA = sep.m_featureB = -1;
sep.m_contact = -1;
@@ -952,7 +952,7 @@ bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const P
sep.m_featureB = p;
sep.m_dist = minDist;
sep.m_axis = planeWorld.GetNormal();
sep.m_separator = Separation::kFeatureB;
sep.m_separator = btSeparation::kFeatureB;
}
// got a separating plane?
@@ -982,7 +982,7 @@ bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const P
{
sep.m_dist = (float)minDist;
sep.m_axis = -planeWorld.GetNormal();
sep.m_separator = Separation::kFeatureA;
sep.m_separator = btSeparation::kFeatureA;
}
}
@@ -1048,7 +1048,7 @@ bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const P
sep.m_featureB = eb;
sep.m_dist = dminA;
sep.m_axis = axis;
sep.m_separator = Separation::kFeatureBoth;
sep.m_separator = btSeparation::kFeatureBoth;
return true;
}
@@ -1065,7 +1065,7 @@ bool Hull::GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const P
// sep.m_featureB = eb;
sep.m_dist = dminA;
sep.m_axis = axis;
// sep.m_separator = Separation::kFeatureBoth;
// sep.m_separator = btSeparation::kFeatureBoth;
}
}

8
Extras/SATConvexCollision/Hull.h
View File

@@ -138,8 +138,8 @@ public:
Point3 GetFaceCentroid(short face) const;
//static void ProcessHullHull(Separation& sep);
static void ProcessHullHull(Separation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB, HullContactCollector* collector);
//static void ProcessHullHull(btSeparation& sep);
static void ProcessHullHull(btSeparation& sep,const Hull& shapeA,const Hull& shapeB,const Transform& trA,const Transform& trB, HullContactCollector* collector);
virtual void ComputeInertia(const Transform& transform, Point3& centerOfMass, Matrix33& inertia, float totalMass) const;
virtual Bounds3 ComputeBounds(const Transform& transform) const;
@@ -153,13 +153,13 @@ public:
/// Clips a face to the back of a plane
static int ClipFace(int numVerts, Point3** ppVtxIn, Point3** ppVtxOut, const Plane& plane);
static bool GetSeparationHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB,
static bool GetSeparationHullHull(btSeparation& sep, const Point3* pVertsA, const Point3* pVertsB,
const Transform& trA, const Transform& trB,
const Hull& hullA,
const Hull& hullB
);
static int AddContactsHullHull(Separation& sep, const Point3* pVertsA, const Point3* pVertsB,
static int AddContactsHullHull(btSeparation& sep, const Point3* pVertsA, const Point3* pVertsB,
const Transform& trA, const Transform& trB,const Hull& hullA,const Hull& hullB,
HullContactCollector* hullContactCollector);

4
Extras/SATConvexCollision/HullContactCollector.h
View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
class Vector3;
class Point3;
class Scalar;
struct Separation;
struct btSeparation;
///HullContactCollector collects the Hull computation to the contact point results
class HullContactCollector
@@ -29,7 +29,7 @@ public:
virtual ~HullContactCollector() {};
virtual int BatchAddContactGroup(const Separation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths)=0;
virtual int BatchAddContactGroup(const btSeparation& sep,int numContacts,const Vector3& normalWorld,const Vector3& tangent,const Point3* positionsWorld,const float* depths)=0;
virtual int GetMaxNumContacts() const = 0;

2
Extras/SATConvexCollision/Shape.h
View File

@@ -26,7 +26,7 @@
struct Separation
struct btSeparation
{
short m_featureA;

46
Extras/quickstep/OdeConstraintSolver.cpp
View File

@@ -20,12 +20,12 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "btContactConstraint.h"
#include "Solve2LinearConstraint.h"
#include "btSolve2LinearConstraint.h"
#include "btContactSolverInfo.h"
#include "Dynamics/BU_Joint.h"
#include "Dynamics/ContactJoint.h"
#include "LinearMath/GenIDebugDraw.h"
#include "LinearMath/btIDebugDraw.h"
#define USE_SOR_SOLVER
@@ -69,13 +69,13 @@ m_erp(0.4f)
//iterative lcp and penalty method
float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
float OdeConstraintSolver::SolveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
m_CurBody = 0;
m_CurJoint = 0;
RigidBody* bodies [MAX_QUICKSTEP_RIGIDBODIES];
btRigidBody* bodies [MAX_QUICKSTEP_RIGIDBODIES];
int numBodies = 0;
BU_Joint* joints [MAX_QUICKSTEP_RIGIDBODIES*4];
@@ -86,11 +86,11 @@ float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numM
int body0=-1,body1=-1;
PersistentManifold* manifold = manifoldPtr[j];
btPersistentManifold* manifold = manifoldPtr[j];
if (manifold->GetNumContacts() > 0)
{
body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies);
body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies);
body0 = ConvertBody((btRigidBody*)manifold->GetBody0(),bodies,numBodies);
body1 = ConvertBody((btRigidBody*)manifold->GetBody1(),bodies,numBodies);
ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer);
}
}
@@ -104,15 +104,15 @@ float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numM
/////////////////////////////////////////////////////////////////////////////////
typedef SimdScalar dQuaternion[4];
typedef btScalar dQuaternion[4];
#define _R(i,j) R[(i)*4+(j)]
void dRfromQ1 (dMatrix3 R, const dQuaternion q)
{
// q = (s,vx,vy,vz)
SimdScalar qq1 = 2.f*q[1]*q[1];
SimdScalar qq2 = 2.f*q[2]*q[2];
SimdScalar qq3 = 2.f*q[3]*q[3];
btScalar qq1 = 2.f*q[1]*q[1];
btScalar qq2 = 2.f*q[2]*q[2];
btScalar qq3 = 2.f*q[3]*q[3];
_R(0,0) = 1.f - qq2 - qq3;
_R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
_R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
@@ -132,7 +132,7 @@ void dRfromQ1 (dMatrix3 R, const dQuaternion q)
int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies)
int OdeConstraintSolver::ConvertBody(btRigidBody* body,btRigidBody** bodies,int& numBodies)
{
if (!body || (body->getInvMass() == 0.f) )
{
@@ -194,13 +194,13 @@ int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& num
static ContactJoint gJointArray[MAX_JOINTS_1];
void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer)
void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints,
btRigidBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
{
manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(),
((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform());
manifold->RefreshContactPoints(((btRigidBody*)manifold->GetBody0())->getCenterOfMassTransform(),
((btRigidBody*)manifold->GetBody1())->getCenterOfMassTransform());
int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
@@ -209,31 +209,31 @@ void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Join
bool swapBodies = (bodyId0 < 0);
RigidBody* body0,*body1;
btRigidBody* body0,*body1;
if (swapBodies)
{
bodyId0 = _bodyId1;
bodyId1 = _bodyId0;
body0 = (RigidBody*)manifold->GetBody1();
body1 = (RigidBody*)manifold->GetBody0();
body0 = (btRigidBody*)manifold->GetBody1();
body1 = (btRigidBody*)manifold->GetBody0();
} else
{
body0 = (RigidBody*)manifold->GetBody0();
body1 = (RigidBody*)manifold->GetBody1();
body0 = (btRigidBody*)manifold->GetBody0();
body1 = (btRigidBody*)manifold->GetBody1();
}
assert(bodyId0 >= 0);
SimdVector3 color(0,1,0);
btVector3 color(0,1,0);
for (i=0;i<numContacts;i++)
{
if (debugDrawer)
{
const ManifoldPoint& cp = manifold->GetContactPoint(i);
const btManifoldPoint& cp = manifold->GetContactPoint(i);
debugDrawer->DrawContactPoint(
cp.m_positionWorldOnB,

14
Extras/quickstep/OdeConstraintSolver.h
View File

@@ -16,14 +16,14 @@ subject to the following restrictions:
#ifndef ODE_CONSTRAINT_SOLVER_H
#define ODE_CONSTRAINT_SOLVER_H
#include "ConstraintSolver.h"
#include "btConstraintSolver.h"
class RigidBody;
class btRigidBody;
class BU_Joint;
/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework
/// It uses the the unmodified version of quickstep solver from the open dynamics project
class OdeConstraintSolver : public ConstraintSolver
class OdeConstraintSolver : public btConstraintSolver
{
private:
@@ -34,9 +34,9 @@ private:
float m_erp;
int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies);
void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer);
int ConvertBody(btRigidBody* body,btRigidBody** bodies,int& numBodies);
void ConvertConstraint(btPersistentManifold* manifold,BU_Joint** joints,int& numJoints,
btRigidBody** bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
public:
@@ -44,7 +44,7 @@ public:
virtual ~OdeConstraintSolver() {}
virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,IDebugDraw* debugDrawer = 0);
virtual float SolveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,btIDebugDraw* debugDrawer = 0);
///setConstraintForceMixing, the cfm adds some positive value to the main diagonal
///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy'

58
Extras/quickstep/SorLcp.cpp
View File

@@ -5,10 +5,10 @@
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* (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 *
* General Public License is included with this library in the *
* 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. *
@@ -28,7 +28,7 @@
// todo: write own successive overrelaxation gauss-seidel, or jacobi iterative solver
#include "LinearMath/SimdScalar.h"
#include "LinearMath/btScalar.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include <math.h>
@@ -55,8 +55,8 @@
////////////////////////////////////////////////////////////////////
//math stuff
typedef SimdScalar dVector4[4];
typedef SimdScalar dMatrix3[4*3];
typedef btScalar dVector4[4];
typedef btScalar dMatrix3[4*3];
#define dInfinity FLT_MAX
@@ -85,7 +85,7 @@ typedef SimdScalar dMatrix3[4*3];
#define REAL float
#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
SimdScalar dDOT1 (const SimdScalar *a, const SimdScalar *b) { return dDOTpq(a,b,1,1); }
btScalar dDOT1 (const btScalar *a, const btScalar *b) { return dDOTpq(a,b,1,1); }
#define dDOT14(a,b) dDOTpq(a,b,1,4)
#define dCROSS(a,op,b,c) \
@@ -171,12 +171,12 @@ extern "C" {
#define ALLOCA dALLOCA16
typedef const SimdScalar *dRealPtr;
typedef SimdScalar *dRealMutablePtr;
#define dRealArray(name,n) SimdScalar name[n];
#define dRealAllocaArray(name,n) SimdScalar *name = (SimdScalar*) ALLOCA ((n)*sizeof(SimdScalar));
typedef const btScalar *dRealPtr;
typedef btScalar *dRealMutablePtr;
#define dRealArray(name,n) btScalar name[n];
#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar));
void dSetZero1 (SimdScalar *a, int n)
void dSetZero1 (btScalar *a, int n)
{
dAASSERT (a && n >= 0);
while (n > 0) {
@@ -185,7 +185,7 @@ void dSetZero1 (SimdScalar *a, int n)
}
}
void dSetValue1 (SimdScalar *a, int n, SimdScalar value)
void dSetValue1 (btScalar *a, int n, btScalar value)
{
dAASSERT (a && n >= 0);
while (n > 0) {
@@ -220,7 +220,7 @@ void dSetValue1 (SimdScalar *a, int n, SimdScalar value)
// compute iMJ = inv(M)*J'
static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
RigidBody * const *body, dRealPtr invI)
btRigidBody * const *body, dRealPtr invI)
{
int i,j;
dRealMutablePtr iMJ_ptr = iMJ;
@@ -228,7 +228,7 @@ static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int
for (i=0; i<m; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
SimdScalar k = body[b1]->getInvMass();
btScalar k = body[b1]->getInvMass();
for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
if (b2 >= 0) {
@@ -330,7 +330,7 @@ static void multiply_J (int m, dRealMutablePtr J, int *jb,
for (i=0; i<m; i++) {
int b1 = jb[i*2];
int b2 = jb[i*2+1];
SimdScalar sum = 0;
btScalar sum = 0;
dRealMutablePtr in_ptr = in + b1*6;
for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
J_ptr += 6;
@@ -358,7 +358,7 @@ static void multiply_J (int m, dRealMutablePtr J, int *jb,
struct IndexError {
SimdScalar error; // error to sort on
btScalar error; // error to sort on
int findex;
int index; // row index
};
@@ -378,7 +378,7 @@ int dRandInt2 (int n)
}
static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * const *body,
static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, btRigidBody * const *body,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
int numiter,float overRelax)
@@ -527,7 +527,7 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * cons
// the constraints are ordered so that all lambda[] values needed have
// already been computed.
if (findex[index] >= 0) {
hi[index] = SimdFabs (hicopy[index] * lambda[findex[index]]);
hi[index] = btFabs (hicopy[index] * lambda[findex[index]]);
lo[index] = -hi[index];
}
@@ -597,10 +597,10 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * cons
void SolveInternal1 (float global_cfm,
float global_erp,
RigidBody * const *body, int nb,
btRigidBody * const *body, int nb,
BU_Joint * const *_joint,
int nj,
const ContactSolverInfo& solverInfo)
const btContactSolverInfo& solverInfo)
{
int numIter = solverInfo.m_numIterations;
@@ -608,7 +608,7 @@ void SolveInternal1 (float global_cfm,
int i,j;
SimdScalar stepsize1 = dRecip(solverInfo.m_timeStep);
btScalar stepsize1 = dRecip(solverInfo.m_timeStep);
// number all bodies in the body list - set their tag values
for (i=0; i<nb; i++)
@@ -755,7 +755,7 @@ void SolveInternal1 (float global_cfm,
dRealAllocaArray (tmp1,nb*6);
// put v/h + invM*fe into tmp1
for (i=0; i<nb; i++) {
SimdScalar body_invMass = body[i]->getInvMass();
btScalar body_invMass = body[i]->getInvMass();
for (j=0; j<3; j++)
tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->getLinearVelocity()[j] * stepsize1;
dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc);
@@ -779,7 +779,7 @@ void SolveInternal1 (float global_cfm,
#ifdef WARM_STARTING
dSetZero1 (lambda,m); //@@@ shouldn't be necessary
for (i=0; i<nj; i++) {
memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(SimdScalar));
memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(btScalar));
}
#endif
@@ -793,7 +793,7 @@ void SolveInternal1 (float global_cfm,
//@@@ note that this doesn't work for contact joints yet, as they are
// recreated every iteration
for (i=0; i<nj; i++) {
memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(SimdScalar));
memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(btScalar));
}
#endif
@@ -803,8 +803,8 @@ void SolveInternal1 (float global_cfm,
// add stepsize * cforce to the body velocity
for (i=0; i<nb; i++) {
SimdVector3 linvel = body[i]->getLinearVelocity();
SimdVector3 angvel = body[i]->getAngularVelocity();
btVector3 linvel = body[i]->getLinearVelocity();
btVector3 angvel = body[i]->getAngularVelocity();
for (j=0; j<3; j++)
linvel[j] += solverInfo.m_timeStep* cforce[i*6+j];
@@ -824,9 +824,9 @@ void SolveInternal1 (float global_cfm,
// add stepsize * invM * fe to the body velocity
for (i=0; i<nb; i++) {
SimdScalar body_invMass = body[i]->getInvMass();
SimdVector3 linvel = body[i]->getLinearVelocity();
SimdVector3 angvel = body[i]->getAngularVelocity();
btScalar body_invMass = body[i]->getInvMass();
btVector3 linvel = body[i]->getLinearVelocity();
btVector3 angvel = body[i]->getAngularVelocity();
for (j=0; j<3; j++)
{

14
Extras/quickstep/SorLcp.h
View File

@@ -5,10 +5,10 @@
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* (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 *
* General Public License is included with this library in the *
* 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. *
@@ -25,16 +25,16 @@
#ifndef SOR_LCP_H
#define SOR_LCP_H
class RigidBody;
class btRigidBody;
class BU_Joint;
#include "LinearMath/SimdScalar.h"
#include "LinearMath/btScalar.h"
struct ContactSolverInfo;
struct btContactSolverInfo;
void SolveInternal1 (float global_cfm,
float global_erp,
RigidBody * const *body, int nb,
BU_Joint * const *_joint, int nj, const ContactSolverInfo& info);
btRigidBody * const *body, int nb,
BU_Joint * const *_joint, int nj, const btContactSolverInfo& info);
int dRandInt2 (int n);

52
Extras/test_BulletOde.cpp
View File

@@ -5,10 +5,10 @@
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of EITHER: *
* (1) The GNU Lesser General Public License as published by the Free *
* (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 *
* General Public License is included with this library in the *
* 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. *
@@ -69,14 +69,14 @@
struct MyObject {
dBodyID body; // the body
CollisionObject collider; //the collisionShape
btCollisionObject collider; //the collisionShape
};
static int num=0; // number of objects in simulation
static int nextobj=0; // next object to recycle if num==NUM
static dWorldID world;
CollisionWorld* collisionWorld = 0;
btCollisionWorld* collisionWorld = 0;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
@@ -88,24 +88,24 @@ static int write_world = 0;
SimdTransform GetTransformFromOde(const dReal* pos,const dReal* rot)
btTransform GetTransformFromOde(const dReal* pos,const dReal* rot)
{
SimdTransform trans;
btTransform trans;
trans.setIdentity();
// rot is pointer to object's rotation matrix, 4*3 format!
SimdMatrix3x3 orn(rot[0],rot[1],rot[2],
btMatrix3x3 orn(rot[0],rot[1],rot[2],
rot[4],rot[5],rot[6],
rot[8],rot[9],rot[10]);
trans.setOrigin(SimdVector3(pos[0],pos[1],pos[2]));
trans.setOrigin(btVector3(pos[0],pos[1],pos[2]));
trans.setBasis(orn);
return trans;
}
void GetOdeFromTransform(const SimdTransform& trans,dReal* pos,dReal* rot)
void GetOdeFromTransform(const btTransform& trans,dReal* pos,dReal* rot)
{
pos[0] = trans.getOrigin().x();
pos[1] = trans.getOrigin().y();
@@ -180,7 +180,7 @@ static void command (int cmd)
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
collisionWorld->RemoveCollisionObject(&obj[i].collider);
obj[i].collider.m_broadphaseHandle = (BroadphaseProxy*)(-1);
obj[i].collider.m_broadphaseHandle = (btBroadphaseProxy*)(-1);
//todo: destroy collider
@@ -210,7 +210,7 @@ static void command (int cmd)
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].collider.m_collisionShape = new BoxShape(SimdVector3(0.5*sides[0],0.5*sides[1],0.5*sides[2]));
obj[i].collider.m_collisionShape = new btBoxShape(btVector3(0.5*sides[0],0.5*sides[1],0.5*sides[2]));
obj[i].collider.m_worldTransform = GetTransformFromOde(dBodyGetPosition(obj[i].body),dBodyGetRotation(obj[i].body));
collisionWorld->AddCollisionObject(&obj[i].collider);
obj[i].collider.m_userPointer = obj[i].body;
@@ -221,7 +221,7 @@ static void command (int cmd)
sides[1] *= 0.2;
sides[2] *= 0.2;
dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].collider.m_collisionShape = new CylinderShapeZ(SimdVector3(sides[0],sides[1],sides[1]));
obj[i].collider.m_collisionShape = new btCylinderShapeZ(btVector3(sides[0],sides[1],sides[1]));
obj[i].collider.m_worldTransform = GetTransformFromOde(dBodyGetPosition(obj[i].body),dBodyGetRotation(obj[i].body));
collisionWorld->AddCollisionObject(&obj[i].collider);
obj[i].collider.m_userPointer = obj[i].body;
@@ -238,7 +238,7 @@ static void command (int cmd)
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].collider.m_collisionShape = new SphereShape(sides[0]);
obj[i].collider.m_collisionShape = new btSphereShape(sides[0]);
obj[i].collider.m_worldTransform = GetTransformFromOde(dBodyGetPosition(obj[i].body),dBodyGetRotation(obj[i].body));
@@ -289,7 +289,7 @@ static void command (int cmd)
// draw a geom
void drawGeom (CollisionObject& collider)//, const dReal *pos, const dReal *R, int show_aabb)
void drawGeom (btCollisionObject& collider)//, const dReal *pos, const dReal *R, int show_aabb)
{
dReal pos[4];
dReal R[16];
@@ -304,7 +304,7 @@ void drawGeom (CollisionObject& collider)//, const dReal *pos, const dReal *R, i
if (type == BOX_SHAPE_PROXYTYPE) {
dVector3 sides;
BoxShape* boxShape = static_cast<BoxShape*>(collider.m_collisionShape);
btBoxShape* boxShape = static_cast<btBoxShape*>(collider.m_collisionShape);
sides[0] = 2.f*boxShape->GetHalfExtents().x();
sides[1] = 2.f*boxShape->GetHalfExtents().y();
sides[2] = 2.f*boxShape->GetHalfExtents().z();
@@ -313,7 +313,7 @@ void drawGeom (CollisionObject& collider)//, const dReal *pos, const dReal *R, i
}
else if (type == SPHERE_SHAPE_PROXYTYPE) {
SphereShape* sphereShape = static_cast<SphereShape*>(collider.m_collisionShape);
btSphereShape* sphereShape = static_cast<btSphereShape*>(collider.m_collisionShape);
dReal radius = sphereShape->GetMargin();
dsDrawSphere (pos,R,radius);
@@ -322,7 +322,7 @@ void drawGeom (CollisionObject& collider)//, const dReal *pos, const dReal *R, i
else if (type == CYLINDER_SHAPE_PROXYTYPE) {
CylinderShapeZ* cylinder = static_cast<CylinderShapeZ*>(collider.m_collisionShape);
btCylinderShapeZ* cylinder = static_cast<btCylinderShapeZ*>(collider.m_collisionShape);
dReal radius = cylinder->GetHalfExtents()[0];
dReal length = 2.f*cylinder->GetHalfExtents()[1];
radius += cylinder->GetMargin();
@@ -381,15 +381,15 @@ static void simLoop (int pause)
for (int i=0;i<collisionWorld->GetDispatcher()->GetNumManifolds();i++)
{
PersistentManifold* manifold = collisionWorld->GetDispatcher()->GetManifoldByIndexInternal(i);
CollisionObject* obj0 = static_cast<CollisionObject*>(manifold->GetBody0());
CollisionObject* obj1 = static_cast<CollisionObject*>(manifold->GetBody1());
btPersistentManifold* manifold = collisionWorld->GetDispatcher()->GetManifoldByIndexInternal(i);
btCollisionObject* obj0 = static_cast<btCollisionObject*>(manifold->GetBody0());
btCollisionObject* obj1 = static_cast<btCollisionObject*>(manifold->GetBody1());
//RefreshContactPoints will update and/or remove existing contactpoints from previous frames
manifold->RefreshContactPoints(obj0->m_worldTransform,obj1->m_worldTransform);
for (int j=0;j<manifold->GetNumContacts();j++)
{
ManifoldPoint& pt = manifold->GetContactPoint(j);
btManifoldPoint& pt = manifold->GetContactPoint(j);
if (pt.GetDistance()<0.f)
{
//report point to ODE
@@ -495,11 +495,11 @@ int main (int argc, char **argv)
//space = dHashSpaceCreate (0);
float bpsize = 1000.f;
AxisSweep3 broadphase(SimdVector3(-bpsize,-bpsize,-bpsize),SimdVector3(bpsize,bpsize,bpsize));
btAxisSweep3 broadphase(btVector3(-bpsize,-bpsize,-bpsize),btVector3(bpsize,bpsize,bpsize));
//SimpleBroadphase broadphase;
CollisionDispatcher dispatcher;
collisionWorld = new CollisionWorld(&dispatcher,&broadphase);
btCollisionDispatcher dispatcher;
collisionWorld = new btCollisionWorld(&dispatcher,&broadphase);
contactgroup = dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
@@ -510,9 +510,9 @@ int main (int argc, char **argv)
dWorldSetContactSurfaceLayer (world,0.001);
//dCreatePlane (space,0,0,1,0);
CollisionObject groundPlane;
btCollisionObject groundPlane;
groundPlane.m_worldTransform.setIdentity();
groundPlane.m_collisionShape = new BoxShape(SimdVector3(50,50,0.04));
groundPlane.m_collisionShape = new btBoxShape(btVector3(50,50,0.04));
groundPlane.m_collisionShape->SetMargin(0.005f);
collisionWorld->AddCollisionObject(&groundPlane);
groundPlane.m_userPointer = 0;