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

moved a few Extras projects into Extras/obsolete.

Browse Source
This commit is contained in:
erwin.coumans
2008-11-17 21:05:22 +00:00
parent fe461296c6
commit 50f475feb5
209 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

360
Extras/obsolete/AlgebraicCCD/BU_AlgebraicPolynomialSolver.cpp Normal file
View File

@@ -0,0 +1,360 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "BU_AlgebraicPolynomialSolver.h"
#include <math.h>
#include <btMinMax.h>
int BU_AlgebraicPolynomialSolver::Solve2Quadratic(btScalar p, btScalar q)
{
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 = 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 (btGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) {
m_roots[0] = - basic_h_local;
return 1;
}
else {
return 0;
}
}
int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(btScalar a,btScalar b, btScalar c)
{
btScalar radical = b * b - 4.0f * a * c;
if(radical >= 0.f)
{
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;
}
return 0;
}
#define cubic_rt(x) \
((x) > 0.0f ? btPow((btScalar)(x), 0.333333333333333333333333f) : \
((x) < 0.0f ? -btPow((btScalar)-(x), 0.333333333333333333333333f) : 0.0f))
/* */
/* this function solves the following cubic equation: */
/* */
/* 3 2 */
/* lead * x + a * x + b * x + c = 0. */
/* */
/* 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(btScalar lead, btScalar a, btScalar b, btScalar c)
{
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 (btEqual(lead, SIMD_EPSILON)) {
/* */
/* we have a x^2 + b x + c = 0 */
/* */
if (btEqual(a, SIMD_EPSILON)) {
/* */
/* we have b x + c = 0 */
/* */
if (btEqual(b, SIMD_EPSILON)) {
if (btEqual(c, SIMD_EPSILON)) {
return -1;
}
else {
return 0;
}
}
else {
m_roots[0] = -c / b;
return 1;
}
}
else {
p = c / a;
q = b / a;
return Solve2QuadraticFull(a,b,c);
}
}
else {
a = a / lead;
b = b / lead;
c = c / lead;
}
}
/* */
/* we substitute x = y - a / 3 in order to eliminate the quadric term. */
/* we get x^3 + p x + q = 0 */
/* */
a /= 3.0f;
u = a * a;
p = b / 3.0f - u;
q = a * (2.0f * u - b) + c;
/* */
/* now use Cardano's formula */
/* */
if (btEqual(p, SIMD_EPSILON)) {
if (btEqual(q, SIMD_EPSILON)) {
/* */
/* one triple root */
/* */
m_roots[0] = -a;
return 1;
}
else {
/* */
/* one real and two complex roots */
/* */
m_roots[0] = cubic_rt(-q) - a;
return 1;
}
}
q /= 2.0f;
delta = p * p * p + q * q;
if (delta > 0.0f) {
/* */
/* one real and two complex roots. note that v = -p / u. */
/* */
u = -q + btSqrt(delta);
u = cubic_rt(u);
m_roots[0] = u - p / u - a;
return 1;
}
else if (delta < 0.0) {
/* */
/* Casus irreducibilis: we have three real roots */
/* */
r = btSqrt(-p);
p *= -r;
r *= 2.0;
phi = btAcos(-q / p) / 3.0f;
dummy = SIMD_2_PI / 3.0f;
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 btScalar root */
/* */
r = cubic_rt(-q);
m_roots[0] = 2.0f * r - a;
m_roots[1] = -r - a;
return 2;
}
}
/* */
/* this function solves the following quartic equation: */
/* */
/* 4 3 2 */
/* lead * x + a * x + b * x + c * x + d = 0. */
/* */
/* 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(btScalar lead, btScalar a, btScalar b, btScalar c, btScalar d)
{
btScalar p, q ,r;
btScalar u, v, w;
int i, num_roots, num_tmp;
//btScalar tmp[2];
if (lead != 1.0) {
/* */
/* transform into normal form: x^4 + a x^3 + b x^2 + c x + d = 0 */
/* */
if (btEqual(lead, SIMD_EPSILON)) {
/* */
/* we have a x^3 + b x^2 + c x + d = 0 */
/* */
if (btEqual(a, SIMD_EPSILON)) {
/* */
/* we have b x^2 + c x + d = 0 */
/* */
if (btEqual(b, SIMD_EPSILON)) {
/* */
/* we have c x + d = 0 */
/* */
if (btEqual(c, SIMD_EPSILON)) {
if (btEqual(d, SIMD_EPSILON)) {
return -1;
}
else {
return 0;
}
}
else {
m_roots[0] = -d / c;
return 1;
}
}
else {
p = c / b;
q = d / b;
return Solve2QuadraticFull(b,c,d);
}
}
else {
return Solve3Cubic(1.0, b / a, c / a, d / a);
}
}
else {
a = a / lead;
b = b / lead;
c = c / lead;
d = d / lead;
}
}
/* */
/* we substitute x = y - a / 4 in order to eliminate the cubic term. */
/* we get: y^4 + p y^2 + q y + r = 0. */
/* */
a /= 4.0f;
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 (btEqual(q, SIMD_EPSILON)) {
/* */
/* biquadratic equation: y^4 + p y^2 + r = 0. */
/* */
num_roots = Solve2Quadratic(p, r);
if (num_roots > 0) {
if (m_roots[0] > 0.0f) {
if (num_roots > 1) {
if ((m_roots[1] > 0.0f) && (m_roots[1] != m_roots[0])) {
u = btSqrt(m_roots[1]);
m_roots[2] = u - a;
m_roots[3] = -u - a;
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 4;
}
else {
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 2;
}
}
else {
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 2;
}
}
}
return 0;
}
else if (btEqual(r, SIMD_EPSILON)) {
/* */
/* no absolute term: y (y^3 + p y + q) = 0. */
/* */
num_roots = Solve3Cubic(1.0, 0.0, p, q);
for (i = 0; i < num_roots; ++i) m_roots[i] -= a;
if (num_roots != -1) {
m_roots[num_roots] = -a;
++num_roots;
}
else {
m_roots[0] = -a;
num_roots = 1;;
}
return num_roots;
}
else {
/* */
/* we solve the resolvent cubic equation */
/* */
num_roots = Solve3Cubic(1.0f, -0.5f * p, -r, 0.5f * r * p - 0.125f * q * q);
if (num_roots == -1) {
num_roots = 1;
m_roots[0] = 0.0f;
}
/* */
/* build two quadric equations */
/* */
w = m_roots[0];
u = w * w - r;
v = 2.0f * w - p;
if (btEqual(u, SIMD_EPSILON))
u = 0.0;
else if (u > 0.0f)
u = btSqrt(u);
else
return 0;
if (btEqual(v, SIMD_EPSILON))
v = 0.0;
else if (v > 0.0f)
v = btSqrt(v);
else
return 0;
if (q < 0.0f) v = -v;
w -= u;
num_roots=Solve2Quadratic(v, w);
for (i = 0; i < num_roots; ++i)
{
m_roots[i] -= a;
}
w += 2.0f *u;
btScalar tmp[2];
tmp[0] = m_roots[0];
tmp[1] = m_roots[1];
num_tmp = Solve2Quadratic(-v, w);
for (i = 0; i < num_tmp; ++i)
{
m_roots[i + num_roots] = tmp[i] - a;
m_roots[i]=tmp[i];
}
return (num_tmp + num_roots);
}
}

45
Extras/obsolete/AlgebraicCCD/BU_AlgebraicPolynomialSolver.h Normal file
View File

@@ -0,0 +1,45 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H
#define BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H
#include "BU_PolynomialSolverInterface.h"
/// BU_AlgebraicPolynomialSolver implements polynomial root finding by analytically solving algebraic equations.
/// Polynomials up to 4rd degree are supported, Cardano's formula is used for 3rd degree
class BU_AlgebraicPolynomialSolver : public BUM_PolynomialSolverInterface
{
public:
BU_AlgebraicPolynomialSolver() {};
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);
btScalar GetRoot(int i) const
{
return m_roots[i];
}
private:
btScalar m_roots[4];
};
#endif //BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H

25
Extras/obsolete/AlgebraicCCD/BU_Collidable.cpp Normal file
View File

@@ -0,0 +1,25 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "BU_Collidable.h"
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
#include <LinearMath/btTransform.h>
#include "BU_MotionStateInterface.h"
BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,btPolyhedralConvexShape& shape,void* userPointer )
:m_motionState(motion),m_shape(shape),m_userPointer(userPointer)
{
}

57
Extras/obsolete/AlgebraicCCD/BU_Collidable.h Normal file
View File

@@ -0,0 +1,57 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BU_COLLIDABLE
#define BU_COLLIDABLE
class btPolyhedralConvexShape;
class BU_MotionStateInterface;
#include <LinearMath/btPoint3.h>
class BU_Collidable
{
public:
BU_Collidable(BU_MotionStateInterface& motion,btPolyhedralConvexShape& shape, void* userPointer);
void* GetUserPointer() const
{
return m_userPointer;
}
BU_MotionStateInterface& GetMotionState()
{
return m_motionState;
}
inline const BU_MotionStateInterface& GetMotionState() const
{
return m_motionState;
}
inline const btPolyhedralConvexShape& GetShape() const
{
return m_shape;
};
private:
BU_MotionStateInterface& m_motionState;
btPolyhedralConvexShape& m_shape;
void* m_userPointer;
};
#endif //BU_COLLIDABLE

581
Extras/obsolete/AlgebraicCCD/BU_CollisionPair.cpp Normal file
View File

@@ -0,0 +1,581 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "BU_CollisionPair.h"
#include "NarrowPhaseCollision/BU_VertexPoly.h"
#include "NarrowPhaseCollision/BU_EdgeEdge.h"
#include "BU_Collidable.h"
#include "BU_MotionStateInterface.h"
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
#include <btMinMax.h>
#include "LinearMath/btTransformUtil.h"
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)
{
}
// if there exists a time-of-impact between any feature_pair (edgeA,edgeB),
// (vertexA,faceB) or (vertexB,faceA) in [0..1], report true and smallest time
/*
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 btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result)
{
btVector3 linvelA,angvelA;
btVector3 linvelB,angvelB;
btTransformUtil::calculateVelocity(fromA,toA,1.f,linvelA,angvelA);
btTransformUtil::calculateVelocity(fromB,toB,1.f,linvelB,angvelB);
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;
btTransform impactTransA;
btTransform impactTransB;
int index=0;
btScalar toiUnscaled=result.m_fraction;
const btScalar toiUnscaledLimit = result.m_fraction;
btTransform a2w;
a2w = fromA;
btTransform b2w = fromB;
/* debugging code
{
const int numvertsB = m_convexB->getNumVertices();
for (int v=0;v<numvertsB;v++)
{
btPoint3 pt;
m_convexB->getVertex(v,pt);
pt = b2w * pt;
char buf[1000];
if (pt.y() < 0.)
{
sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
} else
{
sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
}
}
*/
btTransform b2wp = b2w;
b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
b2wp.setRotation( b2w.getRotation() + angularMotionB);
impactTransB = b2wp;
btTransform a2wp;
a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
a2wp.setRotation(a2w.getRotation()+angularMotionA);
impactTransA = a2wp;
btTransform a2winv;
a2winv = a2w.inverse();
btTransform b2wpinv;
b2wpinv = b2wp.inverse();
btTransform b2winv;
b2winv = b2w.inverse();
btTransform a2wpinv;
a2wpinv = a2wp.inverse();
//Redon's version with concatenated transforms
btTransform relative;
relative = b2w * b2wpinv * a2wp * a2winv;
//relative = a2winv * a2wp * b2wpinv * b2w;
btQuaternion qrel;
relative.getBasis().getRotation(qrel);
btVector3 linvel = relative.getOrigin();
if (linvel.length() < SCREWEPSILON)
{
linvel.setValue(0.,0.,0.);
}
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)
{
angvel.setValue(0.f,0.f,0.f);
}
//Redon's version with concatenated transforms
m_screwing = BU_Screwing(linvel,angvel);
btTransform w2s;
m_screwing.LocalMatrix(w2s);
btTransform s2w;
s2w = w2s.inverse();
//impactTransA = a2w;
//impactTransB = b2w;
bool hit = false;
if (btFuzzyZero(m_screwing.GetS()) && btFuzzyZero(m_screwing.GetW()))
{
//W = 0 , S = 0 , no collision
//toi = 0;
/*
{
const int numvertsB = m_convexB->getNumVertices();
for (int v=0;v<numvertsB;v++)
{
btPoint3 pt;
m_convexB->getVertex(v,pt);
pt = impactTransB * pt;
char buf[1000];
if (pt.y() < 0.)
{
sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
else
{
sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
}
}
*/
return false;//don't continue moving within epsilon
}
#define EDGEEDGE
#ifdef EDGEEDGE
BU_EdgeEdge edgeEdge;
//for all edged in A check agains all edges in B
for (int ea = 0;ea < m_convexA->getNumEdges();ea++)
{
btPoint3 pA0,pA1;
m_convexA->getEdge(ea,pA0,pA1);
pA0= a2w * pA0;//in world space
pA0 = w2s * pA0;//in screwing space
pA1= a2w * pA1;//in world space
pA1 = w2s * pA1;//in screwing space
int numedgesB = m_convexB->getNumEdges();
for (int eb = 0; eb < numedgesB;eb++)
{
{
btPoint3 pB0,pB1;
m_convexB->getEdge(eb,pB0,pB1);
pB0= b2w * pB0;//in world space
pB0 = w2s * pB0;//in screwing space
pB1= b2w * pB1;//in world space
pB1 = w2s * pB1;//in screwing space
btScalar lambda,mu;
toiUnscaled = 1.;
btVector3 edgeDirA(pA1-pA0);
btVector3 edgeDirB(pB1-pB0);
if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu))
{
//printf("edgeedge potential hit\n");
if (toiUnscaled>=0)
{
if (toiUnscaled < toiUnscaledLimit)
{
//inside check is already done by checking the mu and gamma !
btPoint3 vtx = pA0+lambda * (pA1-pA0);
btPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
btPoint3 hitptWorld = s2w * hitpt;
{
if (toiUnscaled < result.m_fraction)
result.m_fraction = toiUnscaled;
hit = true;
btVector3 hitNormal = edgeDirB.cross(edgeDirA);
hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled);
hitNormal.normalize();
//an approximated normal can be calculated by taking the cross product of both edges
//take care of the sign !
btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
btScalar dist = m_screwing.GetU().dot(hitNormalWorld);
if (dist > 0)
hitNormalWorld *= -1;
//todo: this is the wrong point, because b2winv is still at begin of motion
// not at time-of-impact location!
//bhitpt = b2winv * hitptWorld;
// m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld);
}
}
}
}
}
index++;
}
};
#endif //EDGEEDGE
#define VERTEXFACE
#ifdef VERTEXFACE
// for all vertices in A, for each face in B,do vertex-face
{
const int numvertsA = m_convexA->getNumVertices();
for (int v=0;v<numvertsA;v++)
//int v=3;
{
btPoint3 vtx;
m_convexA->getVertex(v,vtx);
vtx = a2w * vtx;//in world space
vtx = w2s * vtx;//in screwing space
const int numplanesB = m_convexB->getNumPlanes();
for (int p = 0 ; p < numplanesB; p++)
//int p=2;
{
{
btVector3 planeNorm;
btPoint3 planeSupport;
m_convexB->getPlane(planeNorm,planeSupport,p);
planeSupport = b2w * planeSupport;//transform to world space
btVector3 planeNormWorld = b2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld;
planeNorm.normalize();
btScalar d = planeSupport.dot(planeNorm);
btVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
BU_VertexPoly vtxApolyB;
toiUnscaled = 1.;
if ((p==2) && (v==6))
{
// printf("%f toiUnscaled\n",toiUnscaled);
}
if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false))
{
if (toiUnscaled >= 0. )
{
//not only collect the first point, get every contactpoint, later we have to check the
//manifold properly!
if (toiUnscaled <= toiUnscaledLimit)
{
// printf("toiUnscaled %f\n",toiUnscaled );
btPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
btVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
btPoint3 hitptWorld = s2w * hitpt;
hitpt = b2winv * hitptWorld;
//vertex has to be 'within' the facet's boundary
if (m_convexB->isInside(hitpt,m_tolerance))
{
// m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld);
if (toiUnscaled < result.m_fraction)
result.m_fraction= toiUnscaled;
hit = true;
}
}
}
}
}
index++;
}
}
}
//
// for all vertices in B, for each face in A,do vertex-face
//copy and pasted from all verts A -> all planes B so potential typos!
//todo: make this into one method with a kind of 'swapped' logic
//
{
const int numvertsB = m_convexB->getNumVertices();
for (int v=0;v<numvertsB;v++)
//int v=0;
{
btPoint3 vtx;
m_convexB->getVertex(v,vtx);
vtx = b2w * vtx;//in world space
/*
char buf[1000];
if (vtx.y() < 0.)
{
sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
*/
vtx = w2s * vtx;//in screwing space
const int numplanesA = m_convexA->getNumPlanes();
for (int p = 0 ; p < numplanesA; p++)
//int p=2;
{
{
btVector3 planeNorm;
btPoint3 planeSupport;
m_convexA->getPlane(planeNorm,planeSupport,p);
planeSupport = a2w * planeSupport;//transform to world space
btVector3 planeNormWorld = a2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld;
planeNorm.normalize();
btScalar d = planeSupport.dot(planeNorm);
btVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
BU_VertexPoly vtxBpolyA;
toiUnscaled = 1.;
if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true))
{
if (toiUnscaled>=0.)
{
if (toiUnscaled < toiUnscaledLimit)
{
btPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
btVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
//btScalar len = hitNormal.length()-1;
//assert( btFuzzyZero(len) );
btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
btPoint3 hitptWorld = s2w * hitpt;
hitpt = a2winv * hitptWorld;
//vertex has to be 'within' the facet's boundary
if (m_convexA->isInside(hitpt,m_tolerance))
{
// m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld);
if (toiUnscaled <result.m_fraction)
result.m_fraction = toiUnscaled;
hit = true;
}
}
}
}
}
}
index++;
}
}
#endif// VERTEXFACE
//the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame
//in addition there are contact points from previous frames
//we have to cleanup the manifold, using an additional epsilon/tolerance
//as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point
//else throw it away
if (hit)
{
//try to avoid numerical drift on close contact
if (result.m_fraction < 0.00001)
{
// printf("toiUnscaledMin< 0.00001\n");
impactTransA = a2w;
impactTransB = b2w;
} else
{
//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);
btQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
ornB.normalize();
impactTransB.setRotation(ornB);
//now transform A
btTransform a2s,a2b;
a2s.mult( w2s , a2w);
a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
a2s.multInverseLeft(w2s,a2s);
a2b.multInverseLeft(b2w, a2s);
//transform by motion B
impactTransA.mult(impactTransB, a2b);
//normalize rotation
btQuaternion orn;
impactTransA.getBasis().getRotation(orn);
orn.normalize();
impactTransA.setBasis(btMatrix3x3(orn));
}
}
/*
{
const int numvertsB = m_convexB->getNumVertices();
for (int v=0;v<numvertsB;v++)
{
btPoint3 pt;
m_convexB->getVertex(v,pt);
pt = impactTransB * pt;
char buf[1000];
if (pt.y() < 0.)
{
sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
else
{
sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
if (debugFile)
fwrite(buf,1,strlen(buf),debugFile);
}
}
}
*/
return hit;
}

54
Extras/obsolete/AlgebraicCCD/BU_CollisionPair.h Normal file
View File

@@ -0,0 +1,54 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BU_COLLISIONPAIR
#define BU_COLLISIONPAIR
#include <NarrowPhaseCollision/BU_Screwing.h>
#include <NarrowPhaseCollision/ConvexCast.h>
#include <btQuaternion.h>
class btPolyhedralConvexShape;
///BU_CollisionPair implements collision algorithm for algebraic time of impact calculation of feature based shapes.
class BU_CollisionPair : public btConvexCast
{
public:
BU_CollisionPair(const btPolyhedralConvexShape* convexA,const btPolyhedralConvexShape* convexB,btScalar tolerance=0.2f);
//toi
virtual bool calcTimeOfImpact(
const btTransform& fromA,
const btTransform& toA,
const btTransform& fromB,
const btTransform& toB,
CastResult& result);
private:
const btPolyhedralConvexShape* m_convexA;
const btPolyhedralConvexShape* m_convexB;
BU_Screwing m_screwing;
btScalar m_tolerance;
};
#endif //BU_COLLISIONPAIR

578
Extras/obsolete/AlgebraicCCD/BU_EdgeEdge.cpp Normal file
View File

@@ -0,0 +1,578 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "BU_EdgeEdge.h"
#include "BU_Screwing.h"
#include <LinearMath/btPoint3.h>
#include <LinearMath/btPoint3.h>
//#include "BU_IntervalArithmeticPolynomialSolver.h"
#include "BU_AlgebraicPolynomialSolver.h"
#define USE_ALGEBRAIC
#ifdef USE_ALGEBRAIC
#define BU_Polynomial BU_AlgebraicPolynomialSolver
#else
#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
#endif
BU_EdgeEdge::BU_EdgeEdge()
{
}
bool BU_EdgeEdge::GetTimeOfImpact(
const BU_Screwing& screwAB,
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;
btScalar lambda;
btScalar mu;
const btScalar w=screwAB.GetW();
const btScalar s=screwAB.GetS();
if (btFuzzyZero(s) &&
btFuzzyZero(w))
{
//no motion, no collision
return false;
}
if (btFuzzyZero(w) )
{
//pure translation W=0, S <> 0
//no trig, f(t)=t
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;
if (mu >=0 && mu <= 1 && lambda >= 0 && lambda <= 1)
{
// single potential collision is
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)
{
hit = true;
lambda1 = lambda;
mu1 = mu;
minTime=t;
}
}
} else
{
//parallel case, not yet
}
} else
{
if (btFuzzyZero(s) )
{
if (btFuzzyZero(u.z()) )
{
if (btFuzzyZero(v.z()) )
{
//u.z()=0,v.z()=0
if (btFuzzyZero(a.z()-c.z()))
{
//printf("NOT YET planar problem, 4 vertex=edge cases\n");
} else
{
//printf("parallel but distinct planes, no collision\n");
return false;
}
} else
{
btScalar mu = (a.z() - c.z())/v.z();
if (0<=mu && mu <= 1)
{
// printf("NOT YET//u.z()=0,v.z()<>0\n");
} else
{
return false;
}
}
} else
{
//u.z()<>0
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");
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
btScalar aa;
btScalar bb;
if (btFuzzyZero(v.x()))
{
aa = v.x()/v.y();
bb= c.x()+ (-c.y() /v.y()) *v.x();
} else
{
//line is c+mu*v;
//x = c.x()+mu*v.x();
//mu = ((x-c.x())/v.x());
//y = c.y()+((x-c.x())/v.x())*v.y();
//y = c.y()+ (-c.x() /v.x()) *v.y() + (x /v.x()) *v.y();
//y = a*x+b,where a = v.y()/v.x(), b= c.y()+ (-c.x() /v.x()) *v.y();
aa = v.y()/v.x();
bb= c.y()+ (-c.x() /v.x()) *v.y();
}
btScalar disc = aa*aa*r2 + r2 - bb*bb;
if (disc <0)
{
//edge doesn't intersect the circle (motion of the vertex)
return false;
}
btScalar rad = btSqrt(r2);
if (btFuzzyZero(disc))
{
btPoint3 intersectPt;
btScalar mu;
//intersectionPoint edge with circle;
if (btFuzzyZero(v.x()))
{
intersectPt.setY( (-2*aa*bb)/(2*(aa*aa+1)));
intersectPt.setX( aa*intersectPt.y()+bb );
mu = ((intersectPt.y()-c.y())/v.y());
} else
{
intersectPt.setX((-2*aa*bb)/(2*(aa*aa+1)));
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);
}
//only one solution
} else
{
//two points...
//intersectionPoint edge with circle;
btPoint3 intersectPt;
//intersectionPoint edge with circle;
if (btFuzzyZero(v.x()))
{
btScalar mu;
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*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)
{
hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
}
} else
{
btScalar mu;
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*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)
{
hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
}
}
}
//int k=0;
} else
{
return false;
}
} else
{
//u.z()<>0,v.z()<>0
//printf("general case with s=0\n");
hit = GetTimeOfImpactbteralCase(screwAB,a,u,c,v,minTime,lambda,mu);
if (hit)
{
lambda1 = lambda;
mu1 = mu;
}
}
}
} else
{
//printf("general case, W<>0,S<>0\n");
hit = GetTimeOfImpactbteralCase(screwAB,a,u,c,v,minTime,lambda,mu);
if (hit)
{
lambda1 = lambda;
mu1 = mu;
}
}
//W <> 0,pure rotation
}
return hit;
}
bool BU_EdgeEdge::GetTimeOfImpactbteralCase(
const BU_Screwing& screwAB,
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;
btScalar coefs[4]={0.f,0.f,0.f,0.f};
BU_Polynomial polynomialSolver;
int numroots = 0;
//btScalar eps=1e-15f;
//btScalar eps2=1e-20f;
btScalar s=screwAB.GetS();
btScalar w = screwAB.GetW();
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 (!btFuzzyZero(v.z()))
{
//Maple Autogenerated C code
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;
btScalar aa,bb,cc,dd;//the coefficients
t1 = v.y()*s; t2 = t1*u.x();
t3 = v.x()*s;
t4 = t3*u.y();
t7 = btTan(w/2.0f);
t8 = 1.0f/t7;
t10 = 1.0f/v.z();
aa = (t2-t4)*t8*t10;
t13 = a.x()*t7;
t14 = u.z()*v.y();
t15 = t13*t14;
t16 = u.x()*v.z();
t17 = a.y()*t7;
t18 = t16*t17;
t19 = u.y()*v.z();
t20 = t13*t19;
t21 = v.y()*u.x();
t22 = c.z()*t7;
t23 = t21*t22;
t24 = v.x()*a.z();
t25 = t7*u.y();
t26 = t24*t25;
t27 = c.y()*t7;
t28 = t16*t27;
t29 = a.z()*t7;
t30 = t21*t29;
t31 = u.z()*v.x();
t32 = t31*t27;
t33 = t31*t17;
t34 = c.x()*t7;
t35 = t34*t19;
t36 = t34*t14;
t39 = v.x()*c.z();
t40 = t39*t25;
t41 = 2.0f*t1*u.y()-t15+t18-t20-t23-t26+t28+t30+t32+t33-t35-t36+2.0f*t3*u.x()+t40;
bb = t41*t8*t10;
t43 = t7*u.x();
t48 = u.y()*v.y();
cc = (-2.0f*t39*t43+2.0f*t24*t43+t4-2.0f*t48*t22+2.0f*t34*t16-2.0f*t31*t13-t2
-2.0f*t17*t14+2.0f*t19*t27+2.0f*t48*t29)*t8*t10;
t63 = -t36+t26+t32-t40+t23+t35-t20+t18-t28-t33+t15-t30;
dd = t63*t8*t10;
coefs[0]=aa;
coefs[1]=bb;
coefs[2]=cc;
coefs[3]=dd;
} else
{
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 = btTan(w/2.0f);
t8 = 1/t7;
t10 = 1/uz;
t13 = ux*az;
t14 = t7*vy;
t15 = t13*t14;
t16 = ax*t7;
t17 = uy*vz;
t18 = t16*t17;
t19 = cx*t7;
t20 = t19*t17;
t21 = vy*uz;
t22 = t19*t21;
t23 = ay*t7;
t24 = vx*uz;
t25 = t23*t24;
t26 = uy*cz;
t27 = t7*vx;
t28 = t26*t27;
t29 = t16*t21;
t30 = cy*t7;
t31 = ux*vz;
t32 = t30*t31;
t33 = ux*cz;
t34 = t33*t14;
t35 = t23*t31;
t36 = t30*t24;
t37 = uy*az;
t38 = t37*t27;
p4 = (-t2+t4)*t8*t10;
p3 = 2.0f*t1*vy+t15-t18-t20-t22+t25+t28-t29+t32-t34+t35+t36-t38+2.0f*t3*vx;
p2 = -2.0f*t33*t27-2.0f*t26*t14-2.0f*t23*t21+2.0f*t37*t14+2.0f*t30*t17+2.0f*t13
*t27+t2-t4+2.0f*t19*t31-2.0f*t16*t24;
t57 = -t22+t29+t36-t25-t32+t34+t35-t28-t15+t20-t18+t38;
p1 = t57*t8*t10;
coefs[0] = p4;
coefs[1] = p3;
coefs[2] = p2;
coefs[1] = p1;
}
numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
for (int i=0;i<numroots;i++)
{
//btScalar tau = roots[i];//polynomialSolver.GetRoot(i);
btScalar tau = polynomialSolver.GetRoot(i);
//check whether mu and lambda are in range [0..1]
if (!btFuzzyZero(v.z()))
{
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)/(btTan(w/2.f)))/v.z();
//double check in original equation
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));
btScalar rhs = c.x()+mu*v.x();
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);
/*btScalar res = coefs[0]*tau*tau*tau+
coefs[1]*tau*tau+
coefs[2]*tau+
coefs[3];*/
//lhs should be rhs !
if (0.<= mu && mu <=1 && 0.<=lambda && lambda <= 1)
{
} else
{
//skip this solution, not really touching
continue;
}
}
btScalar t = 2.f*btAtan(tau)/screwAB.GetW();
//tau = tan (wt/2) so 2*atan (tau)/w
if (t>=0.f && t<minTime)
{
#ifdef STATS_EDGE_EDGE
printf(" ax = %12.12f\n ay = %12.12f\n az = %12.12f\n",a.x(),a.y(),a.z());
printf(" ux = %12.12f\n uy = %12.12f\n uz = %12.12f\n",u.x(),u.y(),u.z());
printf(" cx = %12.12f\n cy = %12.12f\n cz = %12.12f\n",c.x(),c.y(),c.z());
printf(" vx = %12.12f\n vy = %12.12f\n vz = %12.12f\n",v.x(),v.y(),v.z());
printf(" s = %12.12f\n w = %12.12f\n", s, w);
printf(" tau = %12.12f \n lambda = %12.12f \n mu = %f\n",tau,lambda,mu);
printf(" ---------------------------------------------\n");
#endif
// v,u,a,c,s,w
// BU_IntervalArithmeticPolynomialSolver iaSolver;
// int numroots2 = iaSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
minTime = t;
hit = true;
}
}
return hit;
}
//C -S
//S C
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
btPoint3 rotPt1(btCos(rotW)*rotPt.x()-btSin(rotW)*rotPt.y(),
btSin(rotW)*rotPt.x()+btCos(rotW)*rotPt.y(),
0.f);
btVector3 rotVec = rotPt1-rotPt;
btVector3 planeNormal( -rotVec.y() , rotVec.x() ,0.f);
//btPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly
btScalar planeD = planeNormal.dot(rotPt1);
btScalar dist = (planeNormal.dot(intersectPt)-planeD);
hit = (dist >= -0.001);
//if (hit)
{
// minTime = 0;
//calculate the time of impact, using the fact of
//toi = alpha / screwAB.getW();
// cos (alpha) = adjacent/hypothenuse;
//adjacent = dotproduct(ipedge,point);
//hypothenuse = sqrt(r2);
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;
minTime = t;
} else
{
hit = false;
}
}
return hit;
}
bool BU_EdgeEdge::GetTimeOfImpactVertexEdge(
const BU_Screwing& screwAB,
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
)
{
return false;
}

76
Extras/obsolete/AlgebraicCCD/BU_EdgeEdge.h Normal file
View File

@@ -0,0 +1,76 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BU_EDGEEDGE
#define BU_EDGEEDGE
class BU_Screwing;
#include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btVector3.h>
//class BUM_Point2;
#include <LinearMath/btScalar.h>
///BU_EdgeEdge implements algebraic time of impact calculation between two (angular + linear) moving edges.
class BU_EdgeEdge
{
public:
BU_EdgeEdge();
bool GetTimeOfImpact(
const BU_Screwing& screwAB,
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 btPoint3& rotPt, btScalar rotRadius, btScalar rotW,const btPoint3& intersectPt,btScalar& minTime);
bool GetTimeOfImpactbteralCase(
const BU_Screwing& screwAB,
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 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
);
};
#endif //BU_EDGEEDGE

50
Extras/obsolete/AlgebraicCCD/BU_MotionStateInterface.h Normal file
View File

@@ -0,0 +1,50 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BU_MOTIONSTATE
#define BU_MOTIONSTATE
#include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h>
#include <btQuaternion.h>
class BU_MotionStateInterface
{
public:
virtual ~BU_MotionStateInterface(){};
virtual void SetTransform(const btTransform& trans) = 0;
virtual void GetTransform(btTransform& trans) const = 0;
virtual void SetPosition(const btPoint3& position) = 0;
virtual void GetPosition(btPoint3& position) const = 0;
virtual void SetOrientation(const btQuaternion& orientation) = 0;
virtual void GetOrientation(btQuaternion& orientation) const = 0;
virtual void SetBasis(const btMatrix3x3& basis) = 0;
virtual void GetBasis(btMatrix3x3& basis) const = 0;
virtual void SetLinearVelocity(const btVector3& linvel) = 0;
virtual void GetLinearVelocity(btVector3& linvel) const = 0;
virtual void GetAngularVelocity(btVector3& angvel) const = 0;
virtual void SetAngularVelocity(const btVector3& angvel) = 0;
};
#endif //BU_MOTIONSTATE

39
Extras/obsolete/AlgebraicCCD/BU_PolynomialSolverInterface.h Normal file
View File

@@ -0,0 +1,39 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BUM_POLYNOMIAL_SOLVER_INTERFACE
#define BUM_POLYNOMIAL_SOLVER_INTERFACE
#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.
//
class BUM_PolynomialSolverInterface
{
public:
virtual ~BUM_PolynomialSolverInterface() {};
// virtual int Solve2QuadraticFull(btScalar a,btScalar b, btScalar c) = 0;
virtual int Solve3Cubic(btScalar lead, btScalar a, btScalar b, btScalar c) = 0;
virtual int Solve4Quartic(btScalar lead, btScalar a, btScalar b, btScalar c, btScalar d) = 0;
virtual btScalar GetRoot(int i) const = 0;
};
#endif //BUM_POLYNOMIAL_SOLVER_INTERFACE

200
Extras/obsolete/AlgebraicCCD/BU_Screwing.cpp Normal file
View File

@@ -0,0 +1,200 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Stephane Redon / Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "BU_Screwing.h"
BU_Screwing::BU_Screwing(const btVector3& relLinVel,const btVector3& relAngVel) {
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
// s : total amount of translation
// u : vector along the screwing axis (||u||=1)
// o : point on the screwing axis
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=btSqrt(dx*dx+dy*dy+dz*dz);
if (fabs(m_s)<SCREWEPSILON ) {
assert(m_s == 0.);
m_s=0.;
m_u=btPoint3(0.,0.,1.);
m_o=btPoint3(0.,0.,0.);
}
else {
float t=1.f/m_s;
m_u=btPoint3(dx*t,dy*t,dz*t);
m_o=btPoint3(0.f,0.f,0.f);
}
}
else { // there is some rotation
// we compute u
float v(1.f/m_w);
m_u=btPoint3(wx*v,wy*v,wz*v); // normalization
// decomposition of the translation along u and one orthogonal vector
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;
}
btPoint3 n1(t-(m_s*m_u)); // the remaining part (which is orthogonal to u)
// now we have to compute o
//btScalar len = n1.length2();
//(len >= BUM_EPSILON2) {
if (n1[0] || n1[1] || n1[2]) { // n1 is not the zero vector
n1.normalize();
btVector3 n1orth=m_u.cross(n1);
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=btPoint3(0.f,0.f,0.f);
}
}
}
//Then, I need to compute Pa, the matrix from the reference (global) frame to
//the screwing frame :
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.
if ((m_u[0]>SCREWEPSILON)||(m_u[0]<-SCREWEPSILON)||(m_u[1]>SCREWEPSILON)||(m_u[1]<-SCREWEPSILON))
{
// to avoid numerical problems
float n=btSqrt(m_u[0]*m_u[0]+m_u[1]*m_u[1]);
float invn=1.0f/n;
btMatrix3x3 mat;
mat[0][0]=-m_u[1]*invn;
mat[0][1]=m_u[0]*invn;
mat[0][2]=0.f;
mat[1][0]=-m_u[0]*invn*m_u[2];
mat[1][1]=-m_u[1]*invn*m_u[2];
mat[1][2]=n;
mat[2][0]=m_u[0];
mat[2][1]=m_u[1];
mat[2][2]=m_u[2];
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])));
t.setBasis(mat);
}
else {
btMatrix3x3 m;
m[0][0]=1.;
m[1][0]=0.;
m[2][0]=0.;
m[0][1]=0.f;
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(btSign(m_u[2]));
t.setOrigin(btPoint3(
-m_o[0],
-btSign(m_u[2])*m_o[1],
-btSign(m_u[2])*m_o[2]
));
t.setBasis(m);
}
}
//gives interpolated transform for time in [0..1] in screwing frame
btTransform BU_Screwing::InBetweenTransform(const btTransform& tr,btScalar t) const
{
btPoint3 org = tr.getOrigin();
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));
btTransform newtr;
newtr.setOrigin(neworg);
btMatrix3x3 basis = tr.getBasis();
btMatrix3x3 basisorg = tr.getBasis();
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(btMatrix3x3(rot));
return newtr;
}
btScalar BU_Screwing::CalculateF(btScalar t) const
{
btScalar result;
if (!m_w)
{
result = t;
} else
{
result = ( btTan((m_w*t)/2.f) / btTan(m_w/2.f));
}
return result;
}

77
Extras/obsolete/AlgebraicCCD/BU_Screwing.h Normal file
View File

@@ -0,0 +1,77 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B_SCREWING_H
#define B_SCREWING_H
#include <LinearMath/btVector3.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btTransform.h>
#define SCREWEPSILON 0.00001f
///BU_Screwing implements screwing motion interpolation.
class BU_Screwing
{
public:
BU_Screwing(const btVector3& relLinVel,const btVector3& relAngVel);
~BU_Screwing() {
};
btScalar CalculateF(btScalar t) const;
//gives interpolated position for time in [0..1] in screwing frame
inline btPoint3 InBetweenPosition(const btPoint3& pt,btScalar t) const
{
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 btVector3 InBetweenVector(const btVector3& vec,btScalar t) const
{
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
btTransform InBetweenTransform(const btTransform& tr,btScalar t) const;
//gives matrix from global frame into screwing frame
void LocalMatrix(btTransform &t) const;
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;
btVector3 m_u;
btVector3 m_o;
};
#endif //B_SCREWING_H

91
Extras/obsolete/AlgebraicCCD/BU_StaticMotionState.h Normal file
View File

@@ -0,0 +1,91 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BU_STATIC_MOTIONSTATE
#define BU_STATIC_MOTIONSTATE
#include <btCollisionShapes/BU_MotionStateInterface.h>
class BU_StaticMotionState :public BU_MotionStateInterface
{
public:
virtual ~BU_StaticMotionState(){};
virtual void SetTransform(const btTransform& trans)
{
m_trans = trans;
}
virtual void GetTransform(btTransform& trans) const
{
trans = m_trans;
}
virtual void SetPosition(const btPoint3& position)
{
m_trans.setOrigin( position );
}
virtual void GetPosition(btPoint3& position) const
{
position = m_trans.getOrigin();
}
virtual void SetOrientation(const btQuaternion& orientation)
{
m_trans.setRotation( orientation);
}
virtual void GetOrientation(btQuaternion& orientation) const
{
orientation = m_trans.getRotation();
}
virtual void SetBasis(const btMatrix3x3& basis)
{
m_trans.setBasis( basis);
}
virtual void GetBasis(btMatrix3x3& basis) const
{
basis = m_trans.getBasis();
}
virtual void SetLinearVelocity(const btVector3& linvel)
{
m_linearVelocity = linvel;
}
virtual void GetLinearVelocity(btVector3& linvel) const
{
linvel = m_linearVelocity;
}
virtual void SetAngularVelocity(const btVector3& angvel)
{
m_angularVelocity = angvel;
}
virtual void GetAngularVelocity(btVector3& angvel) const
{
angvel = m_angularVelocity;
}
protected:
btTransform m_trans;
btVector3 m_angularVelocity;
btVector3 m_linearVelocity;
};
#endif //BU_STATIC_MOTIONSTATE

159
Extras/obsolete/AlgebraicCCD/BU_VertexPoly.cpp Normal file
View File

@@ -0,0 +1,159 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "BU_VertexPoly.h"
#include "BU_Screwing.h"
#include <LinearMath/btTransform.h>
#include <LinearMath/btPoint3.h>
#include <LinearMath/btVector3.h>
#define USE_ALGEBRAIC
#ifdef USE_ALGEBRAIC
#include "BU_AlgebraicPolynomialSolver.h"
#define BU_Polynomial BU_AlgebraicPolynomialSolver
#else
#include "BU_IntervalArithmeticPolynomialSolver.h"
#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
#endif
inline bool TestFuzzyZero(btScalar x) { return btFabs(x) < 0.0001f; }
BU_VertexPoly::BU_VertexPoly()
{
}
//return true if a collision will occur between [0..1]
//false otherwise. If true, minTime contains the time of impact
bool BU_VertexPoly::GetTimeOfImpact(
const BU_Screwing& screwAB,
const btPoint3& a,
const btVector4& planeEq,
btScalar &minTime,bool swapAB)
{
bool hit = false;
// precondition: s=0 and w= 0 is catched by caller!
if (TestFuzzyZero(screwAB.GetS()) &&
TestFuzzyZero(screwAB.GetW()))
{
return false;
}
//case w<>0 and s<> 0
const btScalar w=screwAB.GetW();
const btScalar s=screwAB.GetS();
btScalar coefs[4];
const btScalar p=planeEq[0];
const btScalar q=planeEq[1];
const btScalar r=planeEq[2];
const btScalar d=planeEq[3];
const btVector3 norm(p,q,r);
BU_Polynomial polynomialSolver;
int numroots = 0;
//btScalar eps=1e-80f;
//btScalar eps2=1e-100f;
if (TestFuzzyZero(screwAB.GetS()) )
{
//S = 0 , W <> 0
//ax^3+bx^2+cx+d=0
coefs[0]=0.;
coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d);
coefs[2]=-2*p*a.y()+2*q*a.x();
coefs[3]=p*a.x()+q*a.y()+r*a.z()-d;
// numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
numroots = polynomialSolver.Solve2QuadraticFull(coefs[1],coefs[2],coefs[3]);
} else
{
if (TestFuzzyZero(screwAB.GetW()))
{
// W = 0 , S <> 0
//pax+qay+r(az+st)=d
btScalar dist = (d - a.dot(norm));
if (TestFuzzyZero(r))
{
if (TestFuzzyZero(dist))
{
// no hit
} else
{
// todo a a' might hit sides of polygon T
//printf("unhandled case, w=0,s<>0,r<>0, a a' might hit sides of polygon T \n");
}
} else
{
btScalar etoi = (dist)/(r*screwAB.GetS());
if (swapAB)
etoi *= -1;
if (etoi >= 0. && etoi <= minTime)
{
minTime = etoi;
hit = true;
}
}
} else
{
//ax^3+bx^2+cx+d=0
//degenerate coefficients mess things up :(
btScalar ietsje = (r*s)/btTan(w/2.f);
if (ietsje*ietsje < 0.01f)
ietsje = 0.f;
coefs[0]=ietsje;//(r*s)/tan(w/2.);
coefs[1]=(-p*a.x()-q*a.y()+r*a.z()-d);
coefs[2]=-2.f*p*a.y()+2.f*q*a.x()+ietsje;//((r*s)/(tan(w/2.)));
coefs[3]=p*a.x()+q*a.y()+r*a.z()-d;
numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
}
}
for (int i=0;i<numroots;i++)
{
btScalar tau = polynomialSolver.GetRoot(i);
btScalar t = 2.f*btAtan(tau)/w;
//tau = tan (wt/2) so 2*atan (tau)/w
if (swapAB)
{
t *= -1.;
}
if (t>=0 && t<minTime)
{
minTime = t;
hit = true;
}
}
return hit;
}

43
Extras/obsolete/AlgebraicCCD/BU_VertexPoly.h Normal file
View File

@@ -0,0 +1,43 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef VERTEX_POLY_H
#define VERTEX_POLY_H
class BU_Screwing;
#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
{
public:
BU_VertexPoly();
bool GetTimeOfImpact(
const BU_Screwing& screwAB,
const btPoint3& vtx,
const btVector4& planeEq,
btScalar &minTime,
bool swapAB);
private:
//cached data (frame coherency etc.) here
};
#endif //VERTEX_POLY_H