int ClosestContact(const ContactPoint& p,const Meshing::TriMesh& mesh,ContactPoint& pclose,Real normalScale)
{
int closest = -1;
Real closestDist2 = Inf;
Triangle3D tri;
Plane3D plane;
for(size_t i=0;i<mesh.tris.size();i++) {
mesh.GetTriangle(i,tri);
//first check distance to supporting plane, since it's a lower bound
tri.getPlane(plane);
Real dxmin = plane.distance(p.x);
Real dn = normalScale*plane.normal.distanceSquared(p.n);
if(dn + Sqr(dxmin) < closestDist2) {
//has potential to be closer than previous
Vector3 cp = tri.closestPoint(p.x);
Real d = cp.distanceSquared(p.x) + dn;
if(d < closestDist2) {
closest = (int)i;
closestDist2 = d;
pclose.x = cp;
pclose.n = plane.normal;
pclose.kFriction = p.kFriction;
}
}
}
return closest;
}
// line (or segment) in 3D vs plane
bool vsLine3D(const Line3D& line, const Plane3D& plane, Point3D *rpoint = NULL, bool check = true)
{
// check if line is parallel with plane
if ( abs( line.vector * plane.normal() ) < epsilon )
return false;
Point3D p = line.point, q = line.point + line.vector;
float normalLen = plane.normal().length();
// get dist (and side) from plane for each point
float distP = plane.Evaluate( p ) / normalLen;
float distQ = plane.Evaluate( q ) / normalLen;
// find t
float t;
if ( ( distP < 0.f && distQ < 0.f ) || ( distP > 0.f && distQ > 0.f ) )
{
if ( check ) return false;
t = abs( distP ) < abs( distQ )
? -abs( distP / ( distP - distQ ) )
: abs( ( distP + distQ ) / ( distP - distQ ) );
}
else
{
t = abs( distP ) / ( abs( distP ) + abs( distQ ) );
}
if ( rpoint ) *rpoint = p + t * line.vector;
return true;
}
Eigen::VectorXd
Line3D::getLine(Plane3D &p1, Plane3D &p2) {
Eigen::VectorXd v(6);
this->m = p1.getPlane() * p2.getPlane().transpose() - p2.getPlane() * p1.getPlane().transpose();
this->plucker_matrix2vector(this->m, v);
this->plucker_vector_swap(v);
return v;
}
// Compute angle between two geometric entities (in radians; use acos)
float AngleBetween(const Line3D& line, const Plane3D& plane)
{
Vector3D u = line.vector ^ plane.normal();
Vector3D v = plane.normal() ^ u;
float angle = acos( v * line.vector );
return ( ( angle < 0.f ) ? ( angle + 2.f * M_PI ) : angle );
}
void PlaneExtents(const Triangle3D& tri,Plane3D& p,Real& dmin,Real& dmax)
{
dmin=dmax=p.distance(tri.a);
Real d=p.distance(tri.b);
if(d<dmin) dmin=d;
else if(d>dmax) dmax=d;
d=p.distance(tri.c);
if(d<dmin) dmin=d;
else if(d>dmax) dmax=d;
}
void Triangle3D::edgeIntersections(const Plane3D& P, Real u[3]) const
{
Real da,db,dc;
da=P.distance(a);
db=P.distance(b);
dc=P.distance(c);
u[0] = SegmentZeroCrossing(da,db);
u[1] = SegmentZeroCrossing(db,dc);
u[2] = SegmentZeroCrossing(dc,da);
}
bool Triangle3D::intersects(const Plane3D& p) const
{
Real minDist,maxDist,d;
minDist = maxDist = p.distance(a);
d = p.distance(b);
if(d < minDist) minDist = d;
else if(d > maxDist) maxDist = d;
d = p.distance(c);
if(d < minDist) minDist = d;
else if(d > maxDist) maxDist = d;
return (minDist <= Zero) && (maxDist >= Zero);
}
void TransformWidget::Drag(int dx,int dy,Camera::Viewport& viewport)
{
dragX += dx;
dragY += dy;
Ray3D r;
viewport.getClickSource(dragX,dragY,r.source);
viewport.getClickVector(dragX,dragY,r.direction);
if(hoverItem < 0) return;
else if(hoverItem == 0) {
Vector3 v;
viewport.getMovementVectorAtDistance(dx,dy,clickDistance,v);
T.t += v;
}
else if(hoverItem <= 3) { //translation
Line3D axisLine;
axisLine.source = clickPos;
axisLine.direction = Vector3(T.R.col(hoverItem-1));
Real t,u;
axisLine.closestPoint(r,t,u);
T.t = clickTransform.t + axisLine.direction*t;
}
else {
Plane3D ringPlane;
Vector3 axis;
if(hoverItem <= 6) axis = Vector3(clickTransform.R.col(hoverItem-4));
else axis = clickAxis;
Vector3 x,y;
GetCanonicalBasis(axis,x,y);
//find rotation to minimize distance from clicked pos to drag ray
Real cx = x.dot(clickPos - T.t);
Real cy = y.dot(clickPos - T.t);
ringPlane.setPointNormal(T.t,axis);
Real t;
bool res=ringPlane.intersectsRay(r,&t);
//odd... no intersection
if(res==false) return;
Vector3 raypos = r.source + t*r.direction - T.t;
Real rx = x.dot(raypos);
Real ry = y.dot(raypos);
if(Sqr(rx) + Sqr(ry) < 1e-5) return;
Real theta = AngleDiff(Atan2(ry,rx),Atan2(cy,cx));
AngleAxisRotation aa;
aa.axis = axis;
aa.angle = theta;
QuaternionRotation qR,qT,qRes;
qR.setAngleAxis(aa);
qT.setMatrix(clickTransform.R);
qRes.mul(qR,qT);
qRes.getMatrix(T.R);
}
Refresh();
}
void ConvexPolyhedron3D::planeExtents(const Plane3D& p,Real& dmin,Real& dmax) const
{
if(numVertices == 0) {
dmin=Inf;
dmax=-Inf;
return;
}
dmin=dmax = p.distance(vertices[0]);
Real d;
for(int i=1; i<numVertices; i++) {
d = p.distance(vertices[i]);
if(d < dmin) dmin=d;
else if(d > dmax) dmax=d;
}
}
//------------------------------------------------------------------------------------------------------
//function that checks if sphere collides with a plane
//------------------------------------------------------------------------------------------------------
bool Sphere3D::IsColliding(const Plane3D& secondPlane) const
{
//make use of Plane3D's plane-sphere collision function
return (secondPlane.IsColliding(*this));
}
int Polygon::ClassifyPolygonToPlane(Plane3D& plane, bool predicate_flag)
{
// Loop over all polygon vertices and count how many vertices
// lie in front of and how many lie behind of the thickened plane
unsigned long numInFront = 0, numBehind = 0;
for (unsigned long i = 0; i < this->_nv; i++) {
// Point *p = _verts[i];
switch (plane.ClassifyPointToPlane(*_verts[i], predicate_flag)) {
//switch (ClassifyPointToPlane(p, plane)) {
case Plane3D::POINT_IN_FRONT_OF_PLANE:
numInFront++;
break;
case Plane3D::POINT_BEHIND_PLANE:
numBehind++;
break;
}
}
/*if (numInFront+numBehind != 3 && !(numInFront==0 && numBehind==0))
return POLYGON_STRADDLING_PLANE;*/
// If vertices on both sides of the plane, the polygon is straddling
if (numBehind != 0 && numInFront != 0)
return POLYGON_STRADDLING_PLANE;
// If one or more vertices in front of the plane and no vertices behind
// the plane, the polygon lies in front of the plane
if (numInFront != 0)
return POLYGON_IN_FRONT_OF_PLANE;
// Ditto, the polygon lies behind the plane if no vertices in front of
// the plane, and one or more vertices behind the plane
if (numBehind != 0)
return POLYGON_BEHIND_PLANE;
// All vertices lie on the plane so the polygon is coplanar with the plane
return POLYGON_COPLANAR_WITH_PLANE;
}
bool Circle3D::intersects(const Line3D& l,Real* _t) const
{
Plane3D p;
getPlane(p);
Real t;
if(p.intersectsLine(l,&t)) {
if(t == Inf) { //line lies in plane
t = l.closestPointParameter(center);
}
if(_t) (*_t)=t;
Point3D pt;
l.eval(t,pt);
return DistanceLEQ(pt,center,radius);
}
return false;
}
void Polygon3D::getPlane(int i,Plane3D& p) const
{
Assert(vertices.size() >= 3);
size_t j=next(i);
size_t k=next(j);
p.setPoints(vertices[i],vertices[j],vertices[k]);
}
void IKGoal::GetClosestGoalTransform(const RigidTransform& T0,RigidTransform& T) const
{
//fill out rotation first
if(rotConstraint == RotFixed) {
GetFixedGoalRotation(T.R);
}
else if(rotConstraint == RotAxis) {
//T.R*localAxis = endRotation
GetMinimalRotation(localAxis,T0.R*endRotation,T.R);
//make it so orthogonal directions perform a rotation similar to T0.R
Vector3 lx,ly,rx,ry,refx;
GetCanonicalBasis(localAxis,lx,ly);
rx = T.R*rx;
ry = T.R*ry;
refx = T0.R*lx;
Real x = dot(refx,rx);
Real y = dot(refx,ry);
//find the rotation about endRotation that gets closer to this
Real theta = Atan2(y,x);
AngleAxisRotation aa;
aa.angle = theta;
aa.axis = T0.R*endRotation;
Matrix3 Rrot;
aa.getMatrix(Rrot);
T.R = Rrot*T.R;
}
else
T.R = T0.R;
T.t = endPosition - T.R*localPosition;
if(posConstraint == PosPlanar) {
//find closest transform on plane to T0.t
Plane3D p;
p.setPointNormal(T.t,direction);
p.project(T0.t,T.t);
}
else if(posConstraint == PosLinear) {
//find closest transform on line to T0.t
Line3D line;
line.source = T.t;
line.direction = direction;
line.closestPoint(T0.t,T.t);
}
else if(posConstraint == PosNone)
T.t = T0.t;
}
Point3D
Line3D::intersectPlane(Plane3D &p) {
Eigen::Vector4d point;
this->plucker_vector2matrix(this->m, this->v);
point = this->m * p.getPlane();
return Point3D(point);
}
Real Polygon3D::maxDistance(const Plane3D& p) const
{
Real dmax=0;
for(size_t i=0;i<vertices.size();i++) {
Real d = p.distance(vertices[i]);
if(d > dmax) dmax=d;
}
return dmax;
}
bool Circle3D::setIntersection(const Sphere3D& s,const Plane3D& p)
{
Real d = p.distance(s.center);
Real absd=Abs(d);
if(absd > s.radius) return false;
axis = p.normal;
//projection of s.center on plane
center = s.center - d*p.normal;
radius = pythag_leg(absd,s.radius);
return true;
}
bool ConvexPolyhedron3D::planeNeg(const Plane3D& p) const
{
Real vpos;
for(int i=0; i<numVertices; i++)
{
vpos = p.distance(vertices[i]);
if(vpos > Zero)
return false;
}
return true;
}
bool TriMesh::PlaneSplits(const Plane3D& p,Real& dmin,Real& dmax) const
{
dmin=Inf;
dmax=-Inf;
Real d;
for(size_t i=0;i<verts.size();i++) {
d = p.distance(verts[i]);
if(d < dmin) dmin=d;
if(d > dmax) dmax=d;
}
return (dmin <= Zero && dmax >= Zero);
}
void Triangle3D::edgeIntersections(const Triangle3D& T, Real u[3]) const
{
Plane3D PT;
T.getPlane(PT);
Real da,db,dc;
da=PT.distance(a);
db=PT.distance(b);
dc=PT.distance(c);
u[0]=u[1]=u[2]=-One;
//check to see if these points are within T's boundaries
Vector3 x;
Vector2 U;
Real ui;
//edge a,b
ui = SegmentZeroCrossing(da,db);
if(ui >= Zero && ui <= One) {
interpolate(a,b,ui,x);
U = T.planeCoords(x);
if(containsPlaneCoords(U)) u[0] = ui;
}
//edge b,c
ui = SegmentZeroCrossing(db,dc);
if(ui >= Zero && ui <= One) {
interpolate(b,c,ui,x);
U = T.planeCoords(x);
if(containsPlaneCoords(U)) u[1] = ui;
}
//edge c,a
ui = SegmentZeroCrossing(dc,da);
if(ui >= Zero && ui <= One) {
interpolate(c,a,ui,x);
U = T.planeCoords(x);
if(containsPlaneCoords(U)) u[2] = ui;
}
}
int Obstacle::findInvisible(const Plane3D & plane) {
double maxDist = 0;
int invisible = -1;
for (int I = 0; I < tops.size(); ++I) {
double dist = plane.dist(tops[I]);
if (invisible == -1 || maxDist < dist) {
maxDist = dist;
invisible = I;
}
}
return invisible;
}
bool ConvexPolyhedron3D::planeSplits(const Plane3D& p) const
{
ClosedInterval x;
x.setEmpty();
Real vpos;
for(int i=0; i<numVertices; i++)
{
vpos = p.distance(vertices[i]);
x.expand(vpos);
if(x.contains(Zero))
return true;
}
return false;
}
bool Circle3D::intersects(const Plane3D& p) const
{
Plane3D cp;
Line3D l;
Point3D lp;
int res=p.allIntersections(cp,l);
switch(res) {
case 0: return false;
case 1: //they intersect in line l
l.closestPoint(center,lp);
return DistanceLEQ(center,lp,radius);
break;
case 2: return true;
default:
fprintf(stderr,"Circle3D::intersects: Shouldn't get here\n");
abort();
}
return false;
}
bool Triangle3D::intersects(const Plane3D& P, Segment3D& S) const
{
Real d[3];
const Point3D* p[3] = {&a,&b,&c};
for(int i=0; i<3; i++) d[i]=P.distance(*p[i]);
//insertion sort
for(int i=1; i<3; i++) {
Real di=d[i];
const Point3D* pi=p[i];
int j=i;
for(; j>0; j--) {
if(d[j-1] <= di) break;
d[j] = d[j-1];
p[j] = p[j-1];
}
d[j] = di;
p[j] = pi;
}
if(!(d[0] <= d[1] && d[1] <= d[2])) {
printf ("AAAACK: %f %f %f\n",d[0],d[1],d[2]);
}
assert(d[0] <= d[1] && d[1] <= d[2]);
if(d[0] > Zero) return false;
if(d[2] < Zero) return false;
Real u;
if(d[1] <= Zero) { //both 0 and 1 are inside p
if(d[0] == d[2]) u = 0;
else u = d[0]/(d[0]-d[2]);
S.a = (One-u)*(*p[0]) + u*(*p[2]);
if(d[1] == d[2]) u = 0;
else u = d[1]/(d[1]-d[2]);
S.b = (One-u)*(*p[1]) + u*(*p[2]);
}
else { //only 0 is inside p
u = d[0]/(d[0]-d[1]);
S.a = (One-u)*(*p[0]) + u*(*p[1]);
u = d[0]/(d[0]-d[2]);
S.b = (One-u)*(*p[0]) + u*(*p[2]);
}
return true;
}
bool TriMesh::Intersects(const Plane3D& p) const
{
vector<Real> d(verts.size());
for(size_t i=0;i<verts.size();i++)
d[i] = p.distance(verts[i]);
Real da,db,dc;
for(size_t i=0;i<tris.size();i++) {
da=d[tris[i].a];
db=d[tris[i].b];
dc=d[tris[i].c];
if(da > Zero) {
if(db <= Zero || dc <= Zero) return true;
}
else if(da < Zero) {
if(db >= Zero || dc >= Zero) return true;
}
else //da==Zero
return true;
}
return false;
}
bool Sphere3D::intersects(const Plane3D& p) const
{
return Abs(p.distance(center)) <= radius;
}
// Determine if two geometric entities are perpendicular.
bool Perpendicular(const Plane3D& plane1, const Plane3D& plane2)
{
return ( plane1.normal() * plane2.normal() <= epsilon );
}
void Circle3D::getPlane(Plane3D& p) const
{
p.setPointNormal(center,axis);
}
bool Triangle3D::intersects(const AABB3D& bb) const
{
//trival accept: contains any point
if(bb.contains(a)||bb.contains(b)||bb.contains(c)) return true;
//trivial reject: bboxes don't intersect
AABB3D tribb;
getAABB(tribb);
if(!bb.intersects(tribb)) return false;
//check for other splitting planes
Plane3D p;
getPlane(p);
if(!p.intersects(bb)) return false;
//check planes orthogonal to edge of tri and edge of bb
ClosedInterval bbInt,triInt;
Vector3 edge;
p.offset = Zero;
//x dir
edge.set(1,0,0);
p.normal.setCross(b-a,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(c-b,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(a-c,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
//y dir
edge.set(0,1,0);
p.normal.setCross(b-a,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(c-b,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(a-c,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
//z dir
edge.set(0,0,1);
p.normal.setCross(b-a,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(c-b,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
p.normal.setCross(a-c,edge);
p.intersects(bb,bbInt.a,bbInt.b);
PlaneExtents(*this,p,triInt.a,triInt.b);
if(!bbInt.intersects(triInt)) return false;
return true;
}
// Determine if two geometric entities are perpendicular.
bool Perpendicular(const Line3D& line, const Plane3D& plane)
{
return ( abs( line.vector * plane.normal() -
line.vector.length() * plane.normal().length() ) <= epsilon );
}