static bool circumCircle(const float* p1, const float* p2, const float* p3,
float* c, float& r)
{
static const float EPS = 1e-6f;
// Calculate the circle relative to p1, to avoid some precision issues.
const float v1[3] = {0,0,0};
float v2[3], v3[3];
rcVsub(v2, p2,p1);
rcVsub(v3, p3,p1);
const float cp = vcross2(v1, v2, v3);
if (fabsf(cp) > EPS)
{
const float v1Sq = vdot2(v1,v1);
const float v2Sq = vdot2(v2,v2);
const float v3Sq = vdot2(v3,v3);
c[0] = (v1Sq*(v2[2]-v3[2]) + v2Sq*(v3[2]-v1[2]) + v3Sq*(v1[2]-v2[2])) / (2*cp);
c[1] = 0;
c[2] = (v1Sq*(v3[0]-v2[0]) + v2Sq*(v1[0]-v3[0]) + v3Sq*(v2[0]-v1[0])) / (2*cp);
r = vdist2(c, v1);
rcVadd(c, c, p1);
return true;
}
rcVcopy(c, p1);
r = 0;
return false;
}
static float distPtTri(const float* p, const float* a, const float* b, const float* c)
{
float v0[3], v1[3], v2[3];
rcVsub(v0, c,a);
rcVsub(v1, b,a);
rcVsub(v2, p,a);
const float dot00 = vdot2(v0, v0);
const float dot01 = vdot2(v0, v1);
const float dot02 = vdot2(v0, v2);
const float dot11 = vdot2(v1, v1);
const float dot12 = vdot2(v1, v2);
// Compute barycentric coordinates
const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
const float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
// If point lies inside the triangle, return interpolated y-coord.
static const float EPS = 1e-4f;
if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS)
{
const float y = a[1] + v0[1]*u + v1[1]*v;
return fabsf(y-p[1]);
}
return FLT_MAX;
}
static float distPtTri(const dtCoordinates& p, const dtCoordinates& a, const dtCoordinates& b, const dtCoordinates& c)
{
dtCoordinates v0, v1, v2;
rcVsub(v0, c,a);
rcVsub(v1, b,a);
rcVsub(v2, p,a);
const float dot00 = vdot2(v0, v0);
const float dot01 = vdot2(v0, v1);
const float dot02 = vdot2(v0, v2);
const float dot11 = vdot2(v1, v1);
const float dot12 = vdot2(v1, v2);
// Compute barycentric coordinates
const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
const float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
// If point lies inside the triangle, return interpolated y-coord.
static const float EPS = 1e-4f;
if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS)
{
const float y = a.Y() + v0.Y()*u + v1.Y()*v;
return fabsf(y-p.Y());
}
return FLT_MAX;
}
static bool circumCircle(const float* p1, const float* p2, const float* p3,
float* c, float& r)
{
static const float EPS = 1e-6f;
const float cp = vcross2(p1, p2, p3);
if (fabsf(cp) > EPS)
{
const float p1Sq = vdot2(p1,p1);
const float p2Sq = vdot2(p2,p2);
const float p3Sq = vdot2(p3,p3);
c[0] = (p1Sq*(p2[2]-p3[2]) + p2Sq*(p3[2]-p1[2]) + p3Sq*(p1[2]-p2[2])) / (2*cp);
c[2] = (p1Sq*(p3[0]-p2[0]) + p2Sq*(p1[0]-p3[0]) + p3Sq*(p2[0]-p1[0])) / (2*cp);
r = vdist2(c, p1);
return true;
}
c[0] = p1[0];
c[2] = p1[2];
r = 0;
return false;
}
static bool barDistSqPointToTri(const float* p, const float* a, const float* b, const float* c)
{
float v0[3], v1[3], v2[3];
rcVsub(v0, c,a);
rcVsub(v1, b,a);
rcVsub(v2, p,a);
const float dot00 = vdot2(v0, v0);
const float dot01 = vdot2(v0, v1);
const float dot02 = vdot2(v0, v2);
const float dot11 = vdot2(v1, v1);
const float dot12 = vdot2(v1, v2);
// Compute barycentric coordinates
float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
float ud = u<0.f ? -u : (u>1.f ? u-1.f : 0.f);
float vd = v<0.f ? -v : (v>1.f ? v-1.f : 0.f);
return ud * ud + vd * vd;
}
static bool circumCircle(const dtCoordinates& p1, const dtCoordinates& p2, const dtCoordinates& p3,
dtCoordinates& c, float& r)
{
static const float EPS = 1e-6f;
const float cp = vcross2(p1, p2, p3);
if (fabsf(cp) > EPS)
{
const float p1Sq = vdot2(p1,p1);
const float p2Sq = vdot2(p2,p2);
const float p3Sq = vdot2(p3,p3);
c.SetX( (p1Sq*(p2.Z()-p3.Z()) + p2Sq*(p3.Z()-p1.Z()) + p3Sq*(p1.Z()-p2.Z())) / (2*cp) );
c.SetZ( (p1Sq*(p3.X()-p2.X()) + p2Sq*(p1.X()-p3.X()) + p3Sq*(p2.X()-p1.X())) / (2*cp) );
r = vdist2(c, p1);
return true;
}
c.SetX( p1.X() );
c.SetZ( p1.Z() );
r = 0;
return false;
}
/*shortest arc
q.w == cos(angle / 2)
q.x == sin(angle / 2) * cross.x
q.y == sin(angle / 2) * cross.y
q.z == sin(angle / 2) * cross.z
dot and cross product of two normalized vectors are:
dot == cos(angle)
cross.x == sin(angle) * Unitperpendicular.x
cross.y == sin(angle) * Unitperpendicular.y
cross.z == sin(angle) * Unitperpendicular.z
*/
void extract_rotation_pseudo_edge(float q[4], float p1x, float p1y, float p1z, float p2x, float p2y, float p2z)
{
float a[3]; /* Axis of rotation */
float p1[3], p2[3];
vset2(p1,p1x,p1y,p1z);
vset2(p2,p2x,p2y,p2z);
/*
* Figure out how much to rotate around that axis.
*/
float d = vdot2(p1,p2);
if(d>=1.0)
{
q[3] = 1.0f;
q[0] = 0.0;
q[1] = 0.0;
q[2]=0.0;
return;
}
if (d < (1e-6f - 1.0f))
{
// Generate an axis
float v1[3] = {1,0,0};float tmp[3];
vcross2(v1,p1,tmp);
if (vlength2(tmp)==0) // pick another if colinear
{
v1[0]=0;v1[1]=1;vcross2(v1,p1,tmp);
}
vnormal2(tmp);
//Half-Way Vector Solution
axis_to_quat(tmp,180,q);
return;
}
//Half-Way Quaternion Solution
vcross2(p1,p2,a);
//float s = sqrt( (1+d)*2 );
//float invs = 1 / s;
q[0] = a[0];// * invs;
q[1] = a[1];//* invs;
q[2] = a[2]; //* invs;
q[3] = 1+d;//s * 0.5f;
}