forked from bartvdbraak/blender
155 lines
3.4 KiB
C++
155 lines
3.4 KiB
C++
/*
|
|
* Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
|
|
*
|
|
* Permission to use, copy, modify, distribute and sell this software
|
|
* and its documentation for any purpose is hereby granted without fee,
|
|
* provided that the above copyright notice appear in all copies.
|
|
* Erwin Coumans makes no representations about the suitability
|
|
* of this software for any purpose.
|
|
* It is provided "as is" without express or implied warranty.
|
|
*/
|
|
|
|
|
|
#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 fabsf(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)/tanf(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*atanf(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;
|
|
}
|