public static ExpVector DirToPlane(this IPlane plane, ExpVector dir)
{
if (plane == null)
{
return(dir);
}
ExpVector result = new ExpVector(0, 0, 0);
result.x = ExpVector.Dot(dir, plane.u);
result.y = ExpVector.Dot(dir, plane.v);
result.z = ExpVector.Dot(dir, plane.n);
return(result);
}
public void GenerateEquations(EquationSystem sys)
{
sys.AddParameters(parameters);
sys.AddEquation(u.Magnitude() - 1.0);
sys.AddEquation(v.Magnitude() - 1.0);
var cross = ExpVector.Cross(u, v);
var dot = ExpVector.Dot(u, v);
sys.AddEquation(Exp.Atan2(cross.Magnitude(), dot) - Math.PI / 2);
sys.AddEquation(n - ExpVector.Cross(u, v));
}
public static Exp angle3d(ExpVector d0, ExpVector d1)
{
return(Exp.Atan2(ExpVector.Cross(d0, d1).Magnitude(), ExpVector.Dot(d0, d1)));
}