public Matrix3x3f transpose()
{
Matrix3x3f ret = new Matrix3x3f();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
ret.e[i, j] = e[j, i];
}
}
return(ret);
}
// Compute the two intersecting lines of a cone with a corner at (0,0,0) with a plane passing by (0,0,0)
// If there is no intersection, return the line on the plane closest to the cone (assumes cone_angle > pi/2)
void IntersectConePlane(Vector3d cone_direction, double cone_angle, Vector3d plane_normal, out Vector3d intersect_1, out Vector3d intersect_2)
{
intersect_1 = Vector3d.zero;
intersect_2 = Vector3d.zero;
cone_direction.Normalize();
plane_normal.Normalize();
// Change the frame of reference:
// x = cone_direction
// y = plane_normal projected on the plane normal to cone_direction
// z = x * y
Vector3d x = cone_direction;
Vector3d z = Vector3d.Cross(x, plane_normal).normalized;
Vector3d y = Vector3d.Cross(z, x);
// transition matrix from the temporary frame to the global frame
Matrix3x3f M_i_tmp = new Matrix3x3f();
for (int i = 0; i < 3; i++)
{
M_i_tmp[i, 0] = (float)x[i];
M_i_tmp[i, 1] = (float)y[i];
M_i_tmp[i, 2] = (float)z[i];
}
Vector3d n = M_i_tmp.transpose() * plane_normal;
double cos_phi = -n.x / (n.y * Math.Tan(cone_angle));
if (Math.Abs(cos_phi) <= 1.0f)
{
intersect_1.x = Math.Cos(cone_angle);
intersect_1.y = Math.Sin(cone_angle) * cos_phi;
intersect_1.z = Math.Sin(cone_angle) * Math.Sqrt(1 - cos_phi * cos_phi);
}
else
{
intersect_1.x = -n.y;
intersect_1.y = n.x;
intersect_1.z = 0.0;
}
intersect_2.x = intersect_1.x;
intersect_2.y = intersect_1.y;
intersect_2.z = -intersect_1.z;
intersect_1 = M_i_tmp * intersect_1;
intersect_2 = M_i_tmp * intersect_2;
}