bool IntrTriangle2Triangle2<Real>::Test ()
{
int i0, i1;
Vector2<Real> dir;
// Test edges of triangle0 for separation.
for (i0 = 0, i1 = 2; i0 < 3; i1 = i0++)
{
// Test axis V0[i1] + t*perp(V0[i0]-V0[i1]), perp(x,y) = (y,-x).
dir.X() = mTriangle0->V[i0].Y() - mTriangle0->V[i1].Y();
dir.Y() = mTriangle0->V[i1].X() - mTriangle0->V[i0].X();
if (WhichSide(mTriangle1->V, mTriangle0->V[i1], dir) > 0)
{
// Triangle1 is entirely on positive side of triangle0 edge.
return false;
}
}
// Test edges of triangle1 for separation.
for (i0 = 0, i1 = 2; i0 < 3; i1 = i0++)
{
// Test axis V1[i1] + t*perp(V1[i0]-V1[i1]), perp(x,y) = (y,-x).
dir.X() = mTriangle1->V[i0].Y() - mTriangle1->V[i1].Y();
dir.Y() = mTriangle1->V[i1].X() - mTriangle1->V[i0].X();
if (WhichSide(mTriangle0->V, mTriangle1->V[i1], dir) > 0)
{
// Triangle0 is entirely on positive side of triangle1 edge.
return false;
}
}
return true;
}
std::pair<Vec3, Vec3> Plane::Clip(const Vec3& pa, const Vec3& pb) const {
if (pa == pb) {
auto whichSide = WhichSide(pa);
if (whichSide >= 0) {
return{ Vec3(FLT_MAX), Vec3(FLT_MAX) };
}
else {
return{ Vec3(-FLT_MAX), Vec3(-FLT_MAX) };
}
}
// Get the projection of the segment onto the plane.
auto direction = pb - pa;
auto ldotv = mNormal.Dot(direction);
// Are the line and plane parallel?
if (ldotv == 0) // line and plane are parallel and maybe coincident
{
auto whichSide = WhichSide(pa);
if (whichSide > 0) {
return{ Vec3(FLT_MAX), Vec3(FLT_MAX) };
}
else if (whichSide == 0.f) {
return{ pa, pb };
}
else {
return{ Vec3(-FLT_MAX), Vec3(-FLT_MAX) };
}
}
// Not parallel so the line intersects. But does the segment intersect?
Real t = -Dot(Vec4(pa)) / ldotv; // ldots / ldotv
// segment does not intersect
if (t < 0 || t > 1){
auto whichSide = WhichSide(pa);
if (whichSide >= 0) {
return{ Vec3(FLT_MAX), Vec3(FLT_MAX) };
}
else {
return{ Vec3(-FLT_MAX), Vec3(-FLT_MAX) };
}
}
auto p = pa + direction * t;
if (ldotv > 0)
return { p, pb };
else
return { pa, p };
}
int main()
{
set_each_analog_state(0,0,0,1,0,0,0,0);
WhichSide();
SetArm();
Start();
Cubes();
}
int main()
{
create_connect();
enable_servos();
set_create_total_angle(0);
set_create_distance(0);
WhichSide();
CenterCamera();
Lift();
}
SeparatePoints2<Real>::SeparatePoints2 (int numPoints0,
const Vector2<Real>* points0, int numPoints1,
const Vector2<Real>* points1, Line2<Real>& separatingLine)
{
// Construct convex hull of point set 0.
ConvexHull2<Real> hull0(numPoints0, (Vector2<Real>*)points0, 0.001f,
false, Query::QT_INT64);
assertion(hull0.GetDimension() == 2,
"Code currently supports only noncollinear points\n");
int numEdges0 = hull0.GetNumSimplices();
const int* edges0 = hull0.GetIndices();
// Construct convex hull of point set 1.
ConvexHull2<Real> hull1(numPoints1,(Vector2<Real>*)points1,0.001f,
false,Query::QT_INT64);
assertion(hull1.GetDimension() == 2,
"Code currently supports only noncollinear points\n");
int numEdges1 = hull1.GetNumSimplices();
const int* edges1 = hull1.GetIndices();
// Test edges of hull 0 for possible separation of points.
int j0, j1, i0, i1, side0, side1;
Vector2<Real> lineNormal;
Real lineConstant;
for (j1 = 0, j0 = numEdges0-1; j1 < numEdges0; j0 = j1++)
{
// Look up edge (assert: i0 != i1 ).
i0 = edges0[j0];
i1 = edges0[j1];
// Compute potential separating line (assert: (xNor,yNor) != (0,0)).
separatingLine.Origin = points0[i0];
separatingLine.Direction = points0[i1] - points0[i0];
separatingLine.Direction.Normalize();
lineNormal = separatingLine.Direction.Perp();
lineConstant = lineNormal.Dot(separatingLine.Origin);
// Determine if hull 1 is on same side of line.
side1 = OnSameSide(lineNormal, lineConstant, numEdges1, edges1,
points1);
if (side1)
{
// Determine which side of line hull 0 lies.
side0 = WhichSide(lineNormal, lineConstant, numEdges0,
edges0, points0);
if (side0*side1 <= 0) // Line separates hulls.
{
mSeparated = true;
return;
}
}
}
// Test edges of hull 1 for possible separation of points.
for (j1 = 0, j0 = numEdges1-1; j1 < numEdges1; j0 = j1++)
{
// Look up edge (assert: i0 != i1 ).
i0 = edges1[j0];
i1 = edges1[j1];
// Compute perpendicular to edge (assert: (xNor,yNor) != (0,0)).
separatingLine.Origin = points1[i0];
separatingLine.Direction = points1[i1] - points1[i0];
separatingLine.Direction.Normalize();
lineNormal = separatingLine.Direction.Perp();
lineConstant = lineNormal.Dot(separatingLine.Origin);
// Determine if hull 0 is on same side of line.
side0 = OnSameSide(lineNormal, lineConstant, numEdges0, edges0,
points0);
if (side0)
{
// Determine which side of line hull 1 lies.
side1 = WhichSide(lineNormal, lineConstant, numEdges1,
edges1, points1);
if (side0*side1 <= 0) // Line separates hulls.
{
mSeparated = true;
return;
}
}
}
mSeparated = false;
}
// seppt3
//---------------------------------------------------------------------------
int SeparatePointSets3D (
// convex hull 0
int n0, const Point pts0[], int fCount0, const Triangle* face0,
// convex hull 1
int n1, const Point pts1[], int fCount1, const Triangle* face1,
// result plane, if result >0
Plane &plane)
{
int i, j0, j1, j2, side0, side1;
//hg was double
float ux, uy, uz, vx, vy, vz;
float xNor,yNor,zNor,c;
// test faces of hull 0 for possible separation of points
for (i = 0; i < fCount0; i++)
{
// lookup face (assert: j0 != j1 && j0 != j2 && j1 != j2)
j0 = face0[i][0];
j1 = face0[i][1];
j2 = face0[i][2];
// compute perpendicular to face (assert: (xNor,yNor,zNor) != (0,0,0))
ux = pts0[j1].x-pts0[j0].x;
uy = pts0[j1].y-pts0[j0].y;
uz = pts0[j1].z-pts0[j0].z;
vx = pts0[j2].x-pts0[j0].x;
vy = pts0[j2].y-pts0[j0].y;
vz = pts0[j2].z-pts0[j0].z;
xNor = uy*vz-uz*vy;
yNor = uz*vx-ux*vz;
zNor = ux*vy-uy*vx;
if (xNor == 0 && yNor == 0 && zNor == 0) {
TRACE("BAD NORMAL in hull !!!!!!\n");
continue;
}
// compute plane constant
c = xNor*pts0[j0].x+yNor*pts0[j0].y+zNor*pts0[j0].z;
// determine if hull 1 is on same side of plane
side1 = OnSameSide(xNor,yNor,zNor,c,fCount1,face1,pts1);
if ( side1 )
{
// determine which side of plane hull 0 lies
side0 = WhichSide(xNor,yNor,zNor,c,fCount0,face0,pts0);
if ( side0*side1 <= 0 ) { // plane separates hulls
plane.Set(xNor,yNor,zNor,c);
return 1;
}
}
}
// test faces of hull 1 for possible separation of points
for (i = 0; i < fCount1; i++)
{
// lookup edge (assert: j0 != j1 && j0 != j2 && j1 != j2)
j0 = face1[i][0];
j1 = face1[i][1];
j2 = face1[i][2];
// compute perpendicular to face (assert: (xNor,yNor,zNor) != (0,0,0))
ux = pts1[j1].x-pts1[j0].x;
uy = pts1[j1].y-pts1[j0].y;
uz = pts1[j1].z-pts1[j0].z;
vx = pts1[j2].x-pts1[j0].x;
vy = pts1[j2].y-pts1[j0].y;
vz = pts1[j2].z-pts1[j0].z;
xNor = uy*vz-uz*vy;
yNor = uz*vx-ux*vz;
zNor = ux*vy-uy*vx;
if (xNor == 0 && yNor == 0 && zNor == 0) {
TRACE("BAD NORMAL in hull !!!!!!\n");
continue;
}
// compute plane constant
c = xNor*pts1[j0].x+yNor*pts1[j0].y+zNor*pts1[j0].z;
// determine if hull 0 is on same side of plane
side0 = OnSameSide(xNor,yNor,zNor,c,fCount0,face0,pts0);
if ( side0 )
{
// determine which side of plane hull 1 lies
side1 = WhichSide(xNor,yNor,zNor,c,fCount1,face1,pts1);
if ( side0*side1 <= 0 ) { // plane separates hulls
/*
// normalize
float l = sqrt(xNor*xNor + yNor*yNor + zNor*zNor);
if (l != 0.0) {
l= 1.0/l;
xNor *= l; yNor *=l; zNor*=l;
}
c = xNor*pts1[j0].x+yNor*pts1[j0].y+zNor*pts1[j0].z;
*/
plane.Set(xNor,yNor,zNor,c);
return 1;
}
}
}
#if 0
// what if two hulls are on same plane ?
// don't do the full test
return (0);
#endif
// build edge list for hull 0
EdgeNode* list0 = 0;
for (i = 0; i < fCount0; i++)
{
// lookup face (assert: j0 != j1 && j0 != j2 && j1 != j2)
j0 = face0[i][0];
j1 = face0[i][1];
j2 = face0[i][2];
AddEdge(list0,j0,j1);
AddEdge(list0,j0,j2);
AddEdge(list0,j1,j2);
}
// build edge list for hull 1
EdgeNode* list1 = 0;
for (i = 0; i < fCount1; i++)
{
// lookup face (assert: j0 != j1 && j0 != j2 && j1 != j2)
j0 = face1[i][0];
j1 = face1[i][1];
j2 = face1[i][2];
AddEdge(list1,j0,j1);
AddEdge(list1,j0,j2);
AddEdge(list1,j1,j2);
}
// Test planes whose normals are cross products of two edges,
// one from each hull.
for (EdgeNode* node0 = list0; node0; node0 = node0->next)
{
// get edge
ux = pts0[node0->v1].x-pts0[node0->v0].x;
uy = pts0[node0->v1].y-pts0[node0->v0].y;
uz = pts0[node0->v1].z-pts0[node0->v0].z;
for (EdgeNode* node1 = list1; node1; node1 = node1->next)
{
vx = pts1[node1->v1].x-pts1[node1->v0].x;
vy = pts1[node1->v1].y-pts1[node1->v0].y;
vz = pts1[node1->v1].z-pts1[node1->v0].z;
// compute plane normal
xNor = uy*vz-uz*vy;
yNor = uz*vx-ux*vz;
zNor = ux*vy-uy*vx;
if (xNor == 0 && yNor == 0 && zNor == 0) {
TRACE("BAD NORMAL in hull !!!!!!\n");
continue;
}
// compute plane constant
c = xNor*pts0[node0->v0].x+yNor*pts0[node0->v0].y+zNor*pts0[node0->v0].z;
// determine if hull 0 is on same side of plane
side0 = OnSameSide(xNor,yNor,zNor,c,fCount0,face0,pts0);
side1 = OnSameSide(xNor,yNor,zNor,c,fCount1,face1,pts1);
if ( side0*side1 < 0 ) { // plane separates hulls
DeleteList(list0); // hg
DeleteList(list1);
plane.Set(xNor,yNor,zNor,c);
return 1;
}
}
}
DeleteList(list0);
DeleteList(list1);
return 0;
}
SeparatePoints3<Real>::SeparatePoints3 (int numPoints0,
const Vector3<Real>* points0, int numPoints1,
const Vector3<Real>* points1, Plane3<Real>& separatingPlane)
{
// Construct convex hull of point set 0.
ConvexHull3<Real> hull0(numPoints0, (Vector3<Real>*)points0, 0.001f,
false, Query::QT_INT64);
// Code does not currently handle point/segment/polygon hull.
assertion(hull0.GetDimension() == 3,
"Code currently supports only noncoplanar points\n");
if (hull0.GetDimension() < 3)
{
return;
}
int numTriangles0 = hull0.GetNumSimplices();
const int* indices0 = hull0.GetIndices();
// Construct convex hull of point set 1.
ConvexHull3<Real> hull1(numPoints1, (Vector3<Real>*)points1, 0.001f,
false, Query::QT_INT64);
// Code does not currently handle point/segment/polygon hull.
assertion(hull1.GetDimension() == 3,
"Code currently supports only noncoplanar points\n");
if (hull1.GetDimension() < 3)
{
return;
}
int numTriangles1 = hull1.GetNumSimplices();
const int* indices1 = hull1.GetIndices();
// Test faces of hull 0 for possible separation of points.
int i, i0, i1, i2, side0, side1;
Vector3<Real> diff0, diff1;
for (i = 0; i < numTriangles0; ++i)
{
// Look up face (assert: i0 != i1 && i0 != i2 && i1 != i2).
i0 = indices0[3*i ];
i1 = indices0[3*i+1];
i2 = indices0[3*i+2];
// Compute potential separating plane (assert: normal != (0,0,0)).
separatingPlane = Plane3<Real>(points0[i0], points0[i1], points0[i2]);
// Determine if hull 1 is on same side of plane.
side1 = OnSameSide(separatingPlane, numTriangles1, indices1, points1);
if (side1)
{
// Determine which side of plane hull 0 lies.
side0 = WhichSide(separatingPlane, numTriangles0, indices0,
points0);
if (side0*side1 <= 0) // Plane separates hulls.
{
mSeparated = true;
return;
}
}
}
// Test faces of hull 1 for possible separation of points.
for (i = 0; i < numTriangles1; ++i)
{
// Look up edge (assert: i0 != i1 && i0 != i2 && i1 != i2).
i0 = indices1[3*i ];
i1 = indices1[3*i+1];
i2 = indices1[3*i+2];
// Compute perpendicular to face (assert: normal != (0,0,0)).
separatingPlane = Plane3<Real>(points1[i0], points1[i1], points1[i2]);
// Determine if hull 0 is on same side of plane.
side0 = OnSameSide(separatingPlane, numTriangles0, indices0, points0);
if (side0)
{
// Determine which side of plane hull 1 lies.
side1 = WhichSide(separatingPlane, numTriangles1, indices1,
points1);
if (side0*side1 <= 0) // Plane separates hulls.
{
mSeparated = true;
return;
}
}
}
// Build edge set for hull 0.
std::set<std::pair<int,int> > edgeSet0;
for (i = 0; i < numTriangles0; ++i)
{
// Look up face (assert: i0 != i1 && i0 != i2 && i1 != i2).
i0 = indices0[3*i ];
i1 = indices0[3*i+1];
i2 = indices0[3*i+2];
edgeSet0.insert(std::make_pair(i0, i1));
edgeSet0.insert(std::make_pair(i0, i2));
edgeSet0.insert(std::make_pair(i1, i2));
}
// Build edge list for hull 1.
std::set<std::pair<int,int> > edgeSet1;
for (i = 0; i < numTriangles1; ++i)
{
// Look up face (assert: i0 != i1 && i0 != i2 && i1 != i2).
i0 = indices1[3*i ];
i1 = indices1[3*i+1];
i2 = indices1[3*i+2];
edgeSet1.insert(std::make_pair(i0, i1));
edgeSet1.insert(std::make_pair(i0, i2));
edgeSet1.insert(std::make_pair(i1, i2));
}
// Test planes whose normals are cross products of two edges, one from
// each hull.
std::set<std::pair<int,int> >::iterator e0iter = edgeSet0.begin();
std::set<std::pair<int,int> >::iterator e0end = edgeSet0.end();
for (/**/; e0iter != e0end; ++e0iter)
{
// Get edge.
diff0 = points0[e0iter->second] - points0[e0iter->first];
std::set<std::pair<int,int> >::iterator e1iter = edgeSet0.begin();
std::set<std::pair<int,int> >::iterator e1end = edgeSet0.end();
for (/**/; e1iter != e1end; ++e1iter)
{
diff1 = points1[e1iter->second] - points1[e1iter->first];
// Compute potential separating plane.
separatingPlane.Normal = diff0.UnitCross(diff1);
separatingPlane.Constant = separatingPlane.Normal.Dot(
points0[e0iter->first]);
// Determine if hull 0 is on same side of plane.
side0 = OnSameSide(separatingPlane, numTriangles0, indices0,
points0);
side1 = OnSameSide(separatingPlane, numTriangles1, indices1,
points1);
if (side0*side1 < 0) // Plane separates hulls.
{
mSeparated = true;
return;
}
}
}
mSeparated = false;
}
