static int overlapSegSeg2d(const float* a, const float* b, const float* c, const float* d)
{
const float a1 = vcross2(a, b, d);
const float a2 = vcross2(a, b, c);
if (a1*a2 < 0.0f)
{
float a3 = vcross2(c, d, a);
float a4 = a3 + a2 - a1;
if (a3 * a4 < 0.0f)
return 1;
}
return 0;
}
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;
}
/*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;
}
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 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;
}
static void completeFacet(rcContext* ctx, const float* pts, int npts, int* edges, int& nedges, const int maxEdges, int& nfaces, int e)
{
static const float EPS = 1e-5f;
int* edge = &edges[e*4];
// Cache s and t.
int s,t;
if (edge[2] == UNDEF)
{
s = edge[0];
t = edge[1];
}
else if (edge[3] == UNDEF)
{
s = edge[1];
t = edge[0];
}
else
{
// Edge already completed.
return;
}
// Find best point on left of edge.
int pt = npts;
float c[3] = {0,0,0};
float r = -1;
for (int u = 0; u < npts; ++u)
{
if (u == s || u == t) continue;
if (vcross2(&pts[s*3], &pts[t*3], &pts[u*3]) > EPS)
{
if (r < 0)
{
// The circle is not updated yet, do it now.
pt = u;
circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
continue;
}
const float d = vdist2(c, &pts[u*3]);
const float tol = 0.001f;
if (d > r*(1+tol))
{
// Outside current circumcircle, skip.
continue;
}
else if (d < r*(1-tol))
{
// Inside safe circumcircle, update circle.
pt = u;
circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
}
else
{
// Inside epsilon circum circle, do extra tests to make sure the edge is valid.
// s-u and t-u cannot overlap with s-pt nor t-pt if they exists.
if (overlapEdges(pts, edges, nedges, s,u))
continue;
if (overlapEdges(pts, edges, nedges, t,u))
continue;
// Edge is valid.
pt = u;
circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
}
}
}
// Add new triangle or update edge info if s-t is on hull.
if (pt < npts)
{
// Update face information of edge being completed.
updateLeftFace(&edges[e*4], s, t, nfaces);
// Add new edge or update face info of old edge.
e = findEdge(edges, nedges, pt, s);
if (e == UNDEF)
addEdge(ctx, edges, nedges, maxEdges, pt, s, nfaces, UNDEF);
else
updateLeftFace(&edges[e*4], pt, s, nfaces);
// Add new edge or update face info of old edge.
e = findEdge(edges, nedges, t, pt);
if (e == UNDEF)
addEdge(ctx, edges, nedges, maxEdges, t, pt, nfaces, UNDEF);
else
updateLeftFace(&edges[e*4], t, pt, nfaces);
nfaces++;
}
else
{
updateLeftFace(&edges[e*4], s, t, HULL);
}
}