/// <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; }