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

Refactoring:

Browse Source 
Moved optional code to Extras: AlgebraicCCD,EPA,quickstep
Moved SimpleBroadphase data to OverlappingPairCache, and derive both SimpleBroadphase and AxisSweep3 from OverlappingPairCache.
Added ParallelPhysicsEnvironment (prepair more parallel mainloop)
Upgraded hardcoded limit from 1024/8192 to 32766/65535 (max objects / max overlapping pairs)
This commit is contained in:
ejcoumans
2006-06-29 20:57:47 +00:00
parent 28a8afe528
commit 9105c3af5a
51 changed files with 7428 additions and 7107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

360
Extras/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 <SimdMinMax.h>
int BU_AlgebraicPolynomialSolver::Solve2Quadratic(SimdScalar p, SimdScalar q)
{
SimdScalar basic_h_local;
SimdScalar 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);
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)) {
m_roots[0] = - basic_h_local;
return 1;
}
else {
return 0;
}
}
int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c)
{
SimdScalar radical = b * b - 4.0f * a * c;
if(radical >= 0.f)
{
SimdScalar sqrtRadical = SimdSqrt(radical);
SimdScalar 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 ? SimdPow((SimdScalar)(x), 0.333333333333333333333333f) : \
((x) < 0.0f ? -SimdPow((SimdScalar)-(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(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c)
{
SimdScalar p, q, r;
SimdScalar delta, u, phi;
SimdScalar dummy;
if (lead != 1.0) {
/* */
/* transform into normal form: x^3 + a x^2 + b x + c = 0 */
/* */
if (SimdEqual(lead, SIMD_EPSILON)) {
/* */
/* we have a x^2 + b x + c = 0 */
/* */
if (SimdEqual(a, SIMD_EPSILON)) {
/* */
/* we have b x + c = 0 */
/* */
if (SimdEqual(b, SIMD_EPSILON)) {
if (SimdEqual(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 (SimdEqual(p, SIMD_EPSILON)) {
if (SimdEqual(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 + SimdSqrt(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 = SimdSqrt(-p);
p *= -r;
r *= 2.0;
phi = SimdAcos(-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;
return 3;
}
else {
/* */
/* one single and one SimdScalar 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(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d)
{
SimdScalar p, q ,r;
SimdScalar u, v, w;
int i, num_roots, num_tmp;
//SimdScalar 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)) {
/* */
/* we have a x^3 + b x^2 + c x + d = 0 */
/* */
if (SimdEqual(a, SIMD_EPSILON)) {
/* */
/* we have b x^2 + c x + d = 0 */
/* */
if (SimdEqual(b, SIMD_EPSILON)) {
/* */
/* we have c x + d = 0 */
/* */
if (SimdEqual(c, SIMD_EPSILON)) {
if (SimdEqual(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 (SimdEqual(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 = SimdSqrt(m_roots[1]);
m_roots[2] = u - a;
m_roots[3] = -u - a;
u = SimdSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 4;
}
else {
u = SimdSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 2;
}
}
else {
u = SimdSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 2;
}
}
}
return 0;
}
else if (SimdEqual(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 (SimdEqual(u, SIMD_EPSILON))
u = 0.0;
else if (u > 0.0f)
u = SimdSqrt(u);
else
return 0;
if (SimdEqual(v, SIMD_EPSILON))
v = 0.0;
else if (v > 0.0f)
v = SimdSqrt(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;
SimdScalar 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/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(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);
SimdScalar GetRoot(int i) const
{
return m_roots[i];
}
private:
SimdScalar m_roots[4];
};
#endif //BU_ALGEBRAIC_POLYNOMIAL_SOLVER_H

25
Extras/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 "CollisionShapes/CollisionShape.h"
#include <SimdTransform.h>
#include "BU_MotionStateInterface.h"
BU_Collidable::BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& shape,void* userPointer )
:m_motionState(motion),m_shape(shape),m_userPointer(userPointer)
{
}

57
Extras/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 PolyhedralConvexShape;
class BU_MotionStateInterface;
#include <SimdPoint3.h>
class BU_Collidable
{
public:
BU_Collidable(BU_MotionStateInterface& motion,PolyhedralConvexShape& 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 PolyhedralConvexShape& GetShape() const
{
return m_shape;
};
private:
BU_MotionStateInterface& m_motionState;
PolyhedralConvexShape& m_shape;
void* m_userPointer;
};
#endif //BU_COLLIDABLE

581
Extras/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 "CollisionShapes/PolyhedralConvexShape.h"
#include <SimdMinMax.h>
#include "SimdTransformUtil.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)),
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 SimdVector3& linearMotionA,const SimdQuaternion& angularMotionA,const SimdVector3& linearMotionB,const SimdQuaternion& angularMotionB, SimdScalar& toi,SimdTransform& impactTransA,SimdTransform& impactTransB)
*/
bool BU_CollisionPair::calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
CastResult& result)
{
SimdVector3 linvelA,angvelA;
SimdVector3 linvelB,angvelB;
SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
SimdTransformUtil::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);
result.m_fraction = 1.f;
SimdTransform impactTransA;
SimdTransform impactTransB;
int index=0;
SimdScalar toiUnscaled=result.m_fraction;
const SimdScalar toiUnscaledLimit = result.m_fraction;
SimdTransform a2w;
a2w = fromA;
SimdTransform b2w = fromB;
/* debugging code
{
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 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);
}
}
}
*/
SimdTransform b2wp = b2w;
b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
b2wp.setRotation( b2w.getRotation() + angularMotionB);
impactTransB = b2wp;
SimdTransform a2wp;
a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
a2wp.setRotation(a2w.getRotation()+angularMotionA);
impactTransA = a2wp;
SimdTransform a2winv;
a2winv = a2w.inverse();
SimdTransform b2wpinv;
b2wpinv = b2wp.inverse();
SimdTransform b2winv;
b2winv = b2w.inverse();
SimdTransform a2wpinv;
a2wpinv = a2wp.inverse();
//Redon's version with concatenated transforms
SimdTransform relative;
relative = b2w * b2wpinv * a2wp * a2winv;
//relative = a2winv * a2wp * b2wpinv * b2w;
SimdQuaternion qrel;
relative.getBasis().getRotation(qrel);
SimdVector3 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]);
if (angvel.length() < SCREWEPSILON)
{
angvel.setValue(0.f,0.f,0.f);
}
//Redon's version with concatenated transforms
m_screwing = BU_Screwing(linvel,angvel);
SimdTransform w2s;
m_screwing.LocalMatrix(w2s);
SimdTransform s2w;
s2w = w2s.inverse();
//impactTransA = a2w;
//impactTransB = b2w;
bool hit = false;
if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW()))
{
//W = 0 , S = 0 , no collision
//toi = 0;
/*
{
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 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++)
{
SimdPoint3 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++)
{
{
SimdPoint3 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
SimdScalar lambda,mu;
toiUnscaled = 1.;
SimdVector3 edgeDirA(pA1-pA0);
SimdVector3 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 !
SimdPoint3 vtx = pA0+lambda * (pA1-pA0);
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
SimdPoint3 hitptWorld = s2w * hitpt;
{
if (toiUnscaled < result.m_fraction)
result.m_fraction = toiUnscaled;
hit = true;
SimdVector3 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 !
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdScalar 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;
{
SimdPoint3 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;
{
{
SimdVector3 planeNorm;
SimdPoint3 planeSupport;
m_convexB->GetPlane(planeNorm,planeSupport,p);
planeSupport = b2w * planeSupport;//transform to world space
SimdVector3 planeNormWorld = b2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld;
planeNorm.normalize();
SimdScalar d = planeSupport.dot(planeNorm);
SimdVector4 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 );
SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdPoint3 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;
{
SimdPoint3 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;
{
{
SimdVector3 planeNorm;
SimdPoint3 planeSupport;
m_convexA->GetPlane(planeNorm,planeSupport,p);
planeSupport = a2w * planeSupport;//transform to world space
SimdVector3 planeNormWorld = a2w.getBasis() * planeNorm;
planeSupport = w2s * planeSupport ; //transform to screwing space
planeNorm = w2s.getBasis() * planeNormWorld;
planeNorm.normalize();
SimdScalar d = planeSupport.dot(planeNorm);
SimdVector4 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)
{
SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
//SimdScalar len = hitNormal.length()-1;
//assert( SimdFuzzyZero(len) );
SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
SimdPoint3 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
{
//SimdScalar 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;
ornB.normalize();
impactTransB.setRotation(ornB);
//now transform A
SimdTransform 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
SimdQuaternion orn;
impactTransA.getBasis().getRotation(orn);
orn.normalize();
impactTransA.setBasis(SimdMatrix3x3(orn));
}
}
/*
{
const int numvertsB = m_convexB->GetNumVertices();
for (int v=0;v<numvertsB;v++)
{
SimdPoint3 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/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 <SimdQuaternion.h>
class PolyhedralConvexShape;
///BU_CollisionPair implements collision algorithm for algebraic time of impact calculation of feature based shapes.
class BU_CollisionPair : public ConvexCast
{
public:
BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance=0.2f);
//toi
virtual bool calcTimeOfImpact(
const SimdTransform& fromA,
const SimdTransform& toA,
const SimdTransform& fromB,
const SimdTransform& toB,
CastResult& result);
private:
const PolyhedralConvexShape* m_convexA;
const PolyhedralConvexShape* m_convexB;
BU_Screwing m_screwing;
SimdScalar m_tolerance;
};
#endif //BU_COLLISIONPAIR

578
Extras/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 <SimdPoint3.h>
#include <SimdPoint3.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 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
)
{
bool hit=false;
SimdScalar lambda;
SimdScalar mu;
const SimdScalar w=screwAB.GetW();
const SimdScalar s=screwAB.GetS();
if (SimdFuzzyZero(s) &&
SimdFuzzyZero(w))
{
//no motion, no collision
return false;
}
if (SimdFuzzyZero(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))
{
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
SimdScalar 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 (SimdFuzzyZero(s) )
{
if (SimdFuzzyZero(u.z()) )
{
if (SimdFuzzyZero(v.z()) )
{
//u.z()=0,v.z()=0
if (SimdFuzzyZero(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
{
SimdScalar 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 (SimdFuzzyZero(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;
//either y=a*x+b, or x = a*x+b...
//depends on whether value v.x() is zero or not
SimdScalar aa;
SimdScalar bb;
if (SimdFuzzyZero(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();
}
SimdScalar 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);
if (SimdFuzzyZero(disc))
{
SimdPoint3 intersectPt;
SimdScalar mu;
//intersectionPoint edge with circle;
if (SimdFuzzyZero(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;
SimdPoint3 intersectPt;
//intersectionPoint edge with circle;
if (SimdFuzzyZero(v.x()))
{
SimdScalar mu;
intersectPt.setY((-2.f*aa*bb+2.f*SimdSqrt(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.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
{
SimdScalar mu;
intersectPt.setX((-2.f*aa*bb+2.f*SimdSqrt(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.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 = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
if (hit)
{
lambda1 = lambda;
mu1 = mu;
}
}
}
} else
{
//printf("general case, W<>0,S<>0\n");
hit = GetTimeOfImpactGeneralCase(screwAB,a,u,c,v,minTime,lambda,mu);
if (hit)
{
lambda1 = lambda;
mu1 = mu;
}
}
//W <> 0,pure rotation
}
return hit;
}
bool BU_EdgeEdge::GetTimeOfImpactGeneralCase(
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
)
{
bool hit = false;
SimdScalar 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();
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();
if (!SimdFuzzyZero(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;
SimdScalar 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);
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
{
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;
t1 = uy*s;
t2 = t1*vx;
t3 = ux*s;
t4 = t3*vy;
t7 = SimdTan(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++)
{
//SimdScalar tau = roots[i];//polynomialSolver.GetRoot(i);
SimdScalar tau = polynomialSolver.GetRoot(i);
//check whether mu and lambda are in range [0..1]
if (!SimdFuzzyZero(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);
lambda = B1/A1;
mu = (a.z()-c.z()+lambda*u.z()+(s*tau)/(SimdTan(w/2.f)))/v.z();
//double check in original equation
SimdScalar 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();
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))-
(ax-ax*tau*tau-2.f*tau*ay);
/*SimdScalar 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;
}
}
SimdScalar t = 2.f*SimdAtan(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 SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& 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(),
0.f);
SimdVector3 rotVec = rotPt1-rotPt;
SimdVector3 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);
SimdScalar 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);
SimdScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
SimdScalar hypo = rotRadius;
SimdScalar alpha = SimdAcos(adjacent/hypo);
SimdScalar 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 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
)
{
return false;
}

76
Extras/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 <SimdTransform.h>
#include <SimdPoint3.h>
#include <SimdVector3.h>
//class BUM_Point2;
#include <SimdScalar.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 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
);
private:
bool Calc2DRotationPointPoint(const SimdPoint3& rotPt, SimdScalar rotRadius, SimdScalar rotW,const SimdPoint3& intersectPt,SimdScalar& minTime);
bool GetTimeOfImpactGeneralCase(
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
);
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
);
};
#endif //BU_EDGEEDGE

50
Extras/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 <SimdTransform.h>
#include <SimdPoint3.h>
#include <SimdQuaternion.h>
class BU_MotionStateInterface
{
public:
virtual ~BU_MotionStateInterface(){};
virtual void SetTransform(const SimdTransform& trans) = 0;
virtual void GetTransform(SimdTransform& trans) const = 0;
virtual void SetPosition(const SimdPoint3& position) = 0;
virtual void GetPosition(SimdPoint3& position) const = 0;
virtual void SetOrientation(const SimdQuaternion& orientation) = 0;
virtual void GetOrientation(SimdQuaternion& orientation) const = 0;
virtual void SetBasis(const SimdMatrix3x3& basis) = 0;
virtual void GetBasis(SimdMatrix3x3& basis) const = 0;
virtual void SetLinearVelocity(const SimdVector3& linvel) = 0;
virtual void GetLinearVelocity(SimdVector3& linvel) const = 0;
virtual void GetAngularVelocity(SimdVector3& angvel) const = 0;
virtual void SetAngularVelocity(const SimdVector3& angvel) = 0;
};
#endif //BU_MOTIONSTATE

39
Extras/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 <SimdScalar.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(SimdScalar a,SimdScalar b, SimdScalar c) = 0;
virtual int Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c) = 0;
virtual int Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d) = 0;
virtual SimdScalar GetRoot(int i) const = 0;
};
#endif //BUM_POLYNOMIAL_SOLVER_INTERFACE

200
Extras/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 SimdVector3& relLinVel,const SimdVector3& 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];
// 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=SimdSqrt(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);
if (fabs(m_s)<SCREWEPSILON ) {
assert(m_s == 0.);
m_s=0.;
m_u=SimdPoint3(0.,0.,1.);
m_o=SimdPoint3(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);
}
}
else { // there is some rotation
// we compute u
float v(1.f/m_w);
m_u=SimdPoint3(wx*v,wy*v,wz*v); // normalization
// decomposition of the translation along u and one orthogonal vector
SimdPoint3 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)
// now we have to compute o
//SimdScalar 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);
float n2x=SimdCos(0.5f*m_w);
float n2y=SimdSin(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);
}
}
}
//Then, I need to compute Pa, the matrix from the reference (global) frame to
//the screwing frame :
void BU_Screwing::LocalMatrix(SimdTransform &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=SimdSqrt(m_u[0]*m_u[0]+m_u[1]*m_u[1]);
float invn=1.0f/n;
SimdMatrix3x3 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(SimdPoint3(
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 {
SimdMatrix3x3 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[2][1]=0.f;
m[0][2]=0.f;
m[1][2]=0.f;
m[2][2]=float(SimdSign(m_u[2]));
t.setOrigin(SimdPoint3(
-m_o[0],
-SimdSign(m_u[2])*m_o[1],
-SimdSign(m_u[2])*m_o[2]
));
t.setBasis(m);
}
}
//gives interpolated transform for time in [0..1] in screwing frame
SimdTransform BU_Screwing::InBetweenTransform(const SimdTransform& tr,SimdScalar t) const
{
SimdPoint3 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),
org.z()+m_s*CalculateF(t));
SimdTransform newtr;
newtr.setOrigin(neworg);
SimdMatrix3x3 basis = tr.getBasis();
SimdMatrix3x3 basisorg = tr.getBasis();
SimdQuaternion rot(SimdVector3(0.,0.,1.),m_w*t);
SimdQuaternion tmpOrn;
tr.getBasis().getRotation(tmpOrn);
rot = rot * tmpOrn;
//to avoid numerical drift, normalize quaternion
rot.normalize();
newtr.setBasis(SimdMatrix3x3(rot));
return newtr;
}
SimdScalar BU_Screwing::CalculateF(SimdScalar t) const
{
SimdScalar result;
if (!m_w)
{
result = t;
} else
{
result = ( SimdTan((m_w*t)/2.f) / SimdTan(m_w/2.f));
}
return result;
}

77
Extras/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 <SimdVector3.h>
#include <SimdPoint3.h>
#include <SimdTransform.h>
#define SCREWEPSILON 0.00001f
///BU_Screwing implements screwing motion interpolation.
class BU_Screwing
{
public:
BU_Screwing(const SimdVector3& relLinVel,const SimdVector3& relAngVel);
~BU_Screwing() {
};
SimdScalar CalculateF(SimdScalar t) const;
//gives interpolated position for time in [0..1] in screwing frame
inline SimdPoint3 InBetweenPosition(const SimdPoint3& pt,SimdScalar 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),
pt.z()+m_s*CalculateF(t));
}
inline SimdVector3 InBetweenVector(const SimdVector3& vec,SimdScalar 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),
vec.z());
}
//gives interpolated transform for time in [0..1] in screwing frame
SimdTransform InBetweenTransform(const SimdTransform& tr,SimdScalar t) const;
//gives matrix from global frame into screwing frame
void LocalMatrix(SimdTransform &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;}
private:
float m_w;
float m_s;
SimdVector3 m_u;
SimdVector3 m_o;
};
#endif //B_SCREWING_H

91
Extras/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 <CollisionShapes/BU_MotionStateInterface.h>
class BU_StaticMotionState :public BU_MotionStateInterface
{
public:
virtual ~BU_StaticMotionState(){};
virtual void SetTransform(const SimdTransform& trans)
{
m_trans = trans;
}
virtual void GetTransform(SimdTransform& trans) const
{
trans = m_trans;
}
virtual void SetPosition(const SimdPoint3& position)
{
m_trans.setOrigin( position );
}
virtual void GetPosition(SimdPoint3& position) const
{
position = m_trans.getOrigin();
}
virtual void SetOrientation(const SimdQuaternion& orientation)
{
m_trans.setRotation( orientation);
}
virtual void GetOrientation(SimdQuaternion& orientation) const
{
orientation = m_trans.getRotation();
}
virtual void SetBasis(const SimdMatrix3x3& basis)
{
m_trans.setBasis( basis);
}
virtual void GetBasis(SimdMatrix3x3& basis) const
{
basis = m_trans.getBasis();
}
virtual void SetLinearVelocity(const SimdVector3& linvel)
{
m_linearVelocity = linvel;
}
virtual void GetLinearVelocity(SimdVector3& linvel) const
{
linvel = m_linearVelocity;
}
virtual void SetAngularVelocity(const SimdVector3& angvel)
{
m_angularVelocity = angvel;
}
virtual void GetAngularVelocity(SimdVector3& angvel) const
{
angvel = m_angularVelocity;
}
protected:
SimdTransform m_trans;
SimdVector3 m_angularVelocity;
SimdVector3 m_linearVelocity;
};
#endif //BU_STATIC_MOTIONSTATE

159
Extras/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 <SimdTransform.h>
#include <SimdPoint3.h>
#include <SimdVector3.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(SimdScalar x) { return SimdFabs(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 SimdPoint3& a,
const SimdVector4& planeEq,
SimdScalar &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 SimdScalar w=screwAB.GetW();
const SimdScalar 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];
const SimdVector3 norm(p,q,r);
BU_Polynomial polynomialSolver;
int numroots = 0;
//SimdScalar eps=1e-80f;
//SimdScalar 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
SimdScalar 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
{
SimdScalar 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 :(
SimdScalar ietsje = (r*s)/SimdTan(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++)
{
SimdScalar tau = polynomialSolver.GetRoot(i);
SimdScalar t = 2.f*SimdAtan(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/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 <SimdTransform.h>
#include <SimdPoint3.h>
#include <SimdScalar.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 SimdPoint3& vtx,
const SimdVector4& planeEq,
SimdScalar &minTime,
bool swapAB);
private:
//cached data (frame coherency etc.) here
};
#endif //VERTEX_POLY_H