101 lines
2.8 KiB
C#
101 lines
2.8 KiB
C#
using System;
|
|
using org.hwo.contracts;
|
|
|
|
using OpenTK;
|
|
|
|
namespace org.budnhead.core
|
|
{
|
|
public static class Geometry
|
|
{
|
|
|
|
/**
|
|
* Intersect Plane and Ray:
|
|
*
|
|
* A + i * (B-A) + j * (C-A) = P + n * V
|
|
* A-P + i * (B-A) + j * (C-A) = n * V
|
|
* i * (B-A) + j * (C-A) - n * V = P-A
|
|
*
|
|
* =>
|
|
*
|
|
* -n * V + i * (B-A) + j * (C-A) = P-A
|
|
*
|
|
* out p : receives the cooefficients of the solved equation (n,i,j)
|
|
*
|
|
**/
|
|
public static bool intersectPlainCoeff(Vector3 P,Vector3 V,Vector3 A,Vector3 B,Vector3 C,out Vector3 p){
|
|
Vector3 BA,CA,PA;
|
|
Vector3 coeff = new Vector3();
|
|
p = coeff;
|
|
|
|
BA = B-A;
|
|
CA = C-A;
|
|
PA = P-A;
|
|
/*
|
|
Console.WriteLine("Interesction:");
|
|
Console.WriteLine("Ray: {0} + i * {1}",P,V);
|
|
Console.WriteLine("A: {0}",A);
|
|
Console.WriteLine("B: {0}",B);
|
|
Console.WriteLine("C: {0}",C);
|
|
*/
|
|
return new BooleanConditional()
|
|
.requires(Linear.Solve4x3(V,BA,CA,PA,out coeff))
|
|
.does(delegate { coeff.X = -coeff.X;})
|
|
.sets(ref p,coeff)
|
|
.isSuccess();
|
|
}
|
|
|
|
public static bool intersectPlain(Vector3 P,Vector3 V,Vector3 A,Vector3 B,Vector3 C,out Vector3 p){
|
|
Vector3 coeff = new Vector3();
|
|
p = new Vector3();
|
|
|
|
return new BooleanConditional()
|
|
.requires(intersectPlainCoeff(P,V,A,B,C,out coeff))
|
|
.assigns(ref p, delegate { return A + (coeff.Y * (B-A)) + (coeff.Z * (C-A)); })
|
|
.isSuccess();
|
|
}
|
|
|
|
public static bool intersectPlainForward(Vector3 P,Vector3 V,Vector3 A,Vector3 B,Vector3 C,out Vector3 p){
|
|
Vector3 coeff = new Vector3();
|
|
p = new Vector3();
|
|
|
|
return new BooleanConditional()
|
|
.requires(intersectPlainCoeff(P,V,A,B,C,out coeff))
|
|
.requires(coeff.X > 0)
|
|
.assigns(ref p, delegate { return P + (coeff.X * V); })
|
|
.isSuccess();
|
|
}
|
|
|
|
public static bool intersectPlainRect(Vector3 P,Vector3 V,Vector3 A,Vector3 B,Vector3 C,out Vector3 p){
|
|
Vector3 coeff = new Vector3();
|
|
p = new Vector3();
|
|
|
|
return new BooleanConditional()
|
|
.requires(intersectPlainCoeff(P,V,A,B,C,out coeff))
|
|
.requires(coeff.Y >= 0)
|
|
.requires(coeff.Z >= 0)
|
|
.requires(coeff.Y <= 1.00001f)
|
|
.requires(coeff.Z <= 1.00001f)
|
|
.assigns(ref p, delegate { return P + (coeff.X * V); })
|
|
.isSuccess();
|
|
}
|
|
|
|
public static bool intersectTriangle(Vector3 P,Vector3 V,Vector3 A,Vector3 B,Vector3 C,out Vector3 p){
|
|
Vector3 coeff = new Vector3();
|
|
p = new Vector3();
|
|
|
|
Console.WriteLine("Check triangle intersection for {0} {1} {2}",A,B,C);
|
|
|
|
return new BooleanConditional()
|
|
.requires(intersectPlainCoeff(P,V,A,B,C,out coeff))
|
|
.requires(coeff.Y >= 0)
|
|
.requires(coeff.Z >= 0)
|
|
.requires(coeff.Y <= 1.00001f)
|
|
.requires(coeff.Z <= 1.00001f)
|
|
.requires(coeff.Y + coeff.Z <= 1.00001f)
|
|
.assigns(ref p, delegate { return A + (coeff.Y * (B-A)) + (coeff.Z * (C-A)); })
|
|
.isSuccess();
|
|
}
|
|
|
|
}
|
|
}
|