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: