/// <summary>
/// Build a Quaternion from the given axis and angle
/// </summary>
/// <param name="axis">The axis to rotate about</param>
/// <param name="angle">The rotation angle in radians</param>
/// <returns></returns>
public static Quaterniond FromAxisAngle(Vector3d axis, double angle)
{
if (axis.sqrMagnitude == 0.0)
{
return(Identity);
}
Quaterniond result = Identity;
angle *= 0.5f;
axis.Normalize();
result.x = axis.x * Math.Sin(angle);
result.y = axis.y * Math.Sin(angle);
result.z = axis.z * Math.Sin(angle);
result.w = (double)System.Math.Cos(angle);
return(result.Normalized());
}
public Quaternion ChooseRotation(Vector3d r, Vector3d f, Vector3d u)
{
// find a reference frame that is close to the given possibly
// non-orthogonal frame
u = u + Vector3d.Normalize (Vector3d.Cross (f, r));
u.Normalize ();
r = Vector3d.Normalize (r - Vector3d.Dot (r, u) * u);
f = Vector3d.Normalize (f - Vector3d.Dot (f, u) * u);
f = Vector3d.Normalize (f + Vector3d.Cross (r, u));
return Quaternion.LookRotation (f, u);
}
static double AngleAboutAxis(Vector3d v1, Vector3d v2, Vector3d axis)
{
axis.Normalize();
v1 = (v1 - Vector3d.Dot(axis, v1) * axis).normalized;
v2 = (v2 - Vector3d.Dot(axis, v2) * axis).normalized;
return Math.Atan2(Vector3d.Dot(axis, Vector3d.Cross(v1, v2)), Vector3d.Dot(v1, v2));
}
void SetNeededVelocity(Vector3d nV)
{
needed_velocity = CFG.MaxNavSpeed;
if(CFG.MaxNavSpeed.Equals(0))
VSL.HorizontalSpeed.SetNeeded(Vector3d.zero);
else if(nV.sqrMagnitude > 0.1 && CFG.MaxNavSpeed > 0)
{
nV.Normalize();
VSL.HorizontalSpeed.SetNeeded(nV*CFG.MaxNavSpeed);
BRC.UpdateBearing((float)VSL.Physics.Bearing(nV));
}
else VSL.HorizontalSpeed.SetNeeded(BRC.ForwardDirection*CFG.MaxNavSpeed);
}
// 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)
static 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;
}
