public static Quat FromTo(Vec3 fromNorm, Vec3 toNorm)
{
Vec3 mf = VecX.Normalize(fromNorm + toNorm);
Quat nq = new Quat(VecX.Cross(mf, toNorm), VecX.Dot(mf, toNorm));
return(nq);
}
public static bool InTriangle(Vec2 p, Triangle <Vec2> tri)
{
double c0 = VecX.Cross(p - tri.p0, p - tri.p1);
double c1 = VecX.Cross(p - tri.p1, p - tri.p2);
double c2 = VecX.Cross(p - tri.p2, p - tri.p0);
return(c0 * c1 >= 0.0 && c1 * c2 >= 0.0 && c2 * c0 >= 0.0);
}
public static bool InConvexPolygon(Vec2 p, IList <Vec2> poly)
{
int lastI = poly.Count - 1;
double lc = VecX.Cross(p - poly[lastI], p - poly[0]);
for (int i = 0; i < lastI; i++)
{
double c = VecX.Cross(p - poly[i], p - poly[i + 1]);
if (c * lc < 0.0)
{
return(false);
}
lc = c;
}
return(true);
}
public static bool LineSegTriangleIntersection(LineSeg <Vec3> s, Triangle <Vec3> t, out Vec3 cp)
{
if (LinePlaneIntersection(s.p0, s.p1 - s.p0, t.p0, VecX.Cross(t.p0 - t.p1, t.p0 - t.p2), out cp))
{
if (VecX.Dot(cp - s.p0, cp - s.p1) <= 0)
{
Vec3 c0 = VecX.Cross(cp - t.p0, cp - t.p1);
Vec3 c1 = VecX.Cross(cp - t.p1, cp - t.p2);
Vec3 c2 = VecX.Cross(cp - t.p2, cp - t.p0);
if (VecX.Dot(c0, c1) >= 0 &&
VecX.Dot(c1, c2) >= 0 &&
VecX.Dot(c2, c0) >= 0)
{
return(true);
}
}
}
return(false);
}
public static bool LineSegsIntersect(LineSeg <Vec2> s0, LineSeg <Vec2> s1)
{
Vec2 vp0 = s0.p1 - s0.p0;
Vec2 vp1 = s1.p0 - s0.p0;
Vec2 vp2 = s1.p1 - s0.p0;
if (VecX.Cross(vp0, vp1) * VecX.Cross(vp0, vp2) > 0)
{
return(false);
}
Vec2 vq0 = s1.p1 - s1.p0;
Vec2 vq1 = s0.p0 - s1.p0;
Vec2 vq2 = s0.p1 - s1.p0;
if (VecX.Cross(vq0, vq1) * VecX.Cross(vq0, vq2) > 0)
{
return(false);
}
return(true);
}
public static bool LineSegsIntersection(LineSeg <Vec2> s0, LineSeg <Vec2> s1, out Vec2 cp)
{
if (LineSegsIntersect(s0, s1))
{
double p0x = s0.p0.x;
double p0y = s0.p0.y;
double q0x = s1.p0.x;
double q0y = s1.p0.y;
double pvx = s0.p1.x - p0x;
double pvy = s0.p1.y - p0y;
double qvx = s1.p1.x - q0x;
double qvy = s1.p1.y - q0y;
double det = VecX.Cross(new Vec2(pvx, pvy), new Vec2(qvx, qvy));
cp.x = -(pvx * qvx * (q0y - p0y) + pvy * qvx * p0x - qvy * pvx * q0x) / det;
cp.y = (pvy * qvy * (q0x - p0x) + pvx * qvy * p0y - qvx * pvy * q0y) / det;
return(true);
}
else
{
cp = default(Vec2);
return(false);
}
}
