public MyVector GetClosestPointOnTriangle(MyVector point)
{
MyVector ab = this.Vertex2 - this.Vertex1;
MyVector ac = this.Vertex3 - this.Vertex1;
MyVector bc = this.Vertex3 - this.Vertex2;
MyVector ap = point - this.Vertex1;
MyVector bp = point - this.Vertex2;
MyVector cp = point - this.Vertex3;
// Compute parametric position s for projection P' of P on AB,
// P' = A + s*AB, s = snom/(snom+sdenom)
double snom = MyVector.Dot(ap, ab);
double sdenom = MyVector.Dot(bp, this.Vertex1 - this.Vertex2);
// Compute parametric position t for projection P' of P on AC,
// P' = A + t*AC, s = tnom/(tnom+tdenom)
double tnom = MyVector.Dot(ap, ac);
double tdenom = MyVector.Dot(cp, this.Vertex1 - this.Vertex3);
if (snom <= 0d && tnom <= 0d)
{
return(this.Vertex1); // Vertex region early out
}
// Compute parametric position u for projection P' of P on BC,
// P' = B + u*BC, u = unom/(unom+udenom)
double unom = MyVector.Dot(bp, bc);
double udenom = MyVector.Dot(cp, this.Vertex2 - this.Vertex3);
if (sdenom <= 0d && unom <= 0d)
{
return(this.Vertex2); // Vertex region early out
}
if (tdenom <= 0d && udenom <= 0d)
{
return(this.Vertex3); // Vertex region early out
}
MyVector pa = this.Vertex1 - point;
MyVector pb = this.Vertex2 - point;
// P is outside (or on) AB if the triple scalar product [N PA PB] <= 0
MyVector n = MyVector.Cross(ab, ac);
double vc = MyVector.Dot(n, MyVector.Cross(pa, pb));
// If P outside AB and within feature region of AB,
// return projection of P onto AB
if (vc <= 0d && snom >= 0d && sdenom >= 0d)
{
return(this.Vertex1 + snom / (snom + sdenom) * ab);
}
MyVector pc = this.Vertex3 - point;
// P is outside (or on) BC if the triple scalar product [N PB PC] <= 0
double va = MyVector.Dot(n, MyVector.Cross(pb, pc));
// If P outside BC and within feature region of BC,
// return projection of P onto BC
if (va <= 0d && unom >= 0d && udenom >= 0d)
{
return(this.Vertex2 + unom / (unom + udenom) * bc);
}
// P is outside (or on) CA if the triple scalar product [N PC PA] <= 0
double vb = MyVector.Dot(n, MyVector.Cross(pc, pa));
// If P outside CA and within feature region of CA,
// return projection of P onto CA
if (vb <= 0d && tnom >= 0d && tdenom >= 0d)
{
return(this.Vertex1 + tnom / (tnom + tdenom) * ac);
}
// P must project inside face region. Compute Q using barycentric coordinates
double u = va / (va + vb + vc);
double v = vb / (va + vb + vc);
double w = 1d - u - v; // = vc / (va + vb + vc)
return(u * this.Vertex1 + v * this.Vertex2 + w * this.Vertex3);
}