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;
}
Exemple #5
0
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);
	}
}