Vector3d getPlaneIntersection( const Hyperplane<double,3> &plane1, const Hyperplane<double,3> &plane2, double lambda)
{
double dot = plane1.normal().dot(plane2.normal());
Vector3d cross = plane1.normal().cross(plane2.normal());
double c1 = (plane1.offset() - plane2.offset()*dot )/(1.0-dot*dot);
double c2 = (plane2.offset() - plane1.offset()*dot )/(1.0-dot*dot);
Vector3d r = (c1*plane1.normal() + c2*plane2.normal() ) + lambda*(cross);
return r;
}
Vector3d getReflected(const Vector3d &p, const Hyperplane<double,3> &plane)
{ Vector3d r = p- 2*plane.signedDistance(p)*plane.normal();
return r;
}