Пример #1
0
		/// <summary>
		/// Multiplies two quaternions, storing the result in out. The resulting
		/// quaternion will have the same effect as the combination of p and q,
		/// ie. when applied to a point, (point * out) = ((point * p) * q). Any
		/// number of rotations can be concatenated in this way. Note that quaternion
		/// multiplication is not commutative, ie. quat_mul(p, q) != quat_mul(q, p).
		/// </summary>
		public Quaternion Multiply (Quaternion q)
		{
			Quaternion ret;

			/* qp = ww' - v.v' + vxv' + wv' + w'v */

			/* w" = ww' - xx' - yy' - zz' */
			ret.w = (w * q.w) - (x * q.x) -	(y * q.y) -	(z * q.z);

			/* x" = wx' + xw' + yz' - zy' */
			ret.x = (w * q.x) + (x * q.w) +  (y * q.z) -(z * q.y);

		/* y" = wy' + yw' + zx' - xz' */
			ret.y = (w * q.y) + (y * q.w) + (z * q.x) - (x * q.z);

			/* z" = wz' + zw' + xy' - yx' */
			ret.z = w * q.z + z * q.w + x * q.y - y * q.x;

			return ret;
		}
Пример #2
0
		/// <summary>
		/// Constructs a quaternion that represents a rotation between 'from' and
		/// 'to'. The argument 't' can be anything between 0 and 1 and represents
		/// where between from and to the result will be.  0 returns 'from', 1
		/// returns 'to', and 0.5 will return a rotation exactly in between. The
		/// result is copied to 'out'.
		/// 
		/// The variable 'how' can be any one of the following:
		/// 
		/// qshort - This equivalent quat_interpolate, the rotation will be less than 180 degrees
		/// qlong  - rotation will be greater than 180 degrees
		/// qcw    - rotation will be clockwise when viewed from above
		/// qccw   - rotation will be counterclockwise when viewed from above
		/// quser  - the quaternions are interpolated exactly as given
		/// </summary>
		public static Quaternion Slerp(Quaternion from, Quaternion to, float t, SlerpType how)
		{
			Quaternion q;
			Quaternion to2;
			double angle;
			double cos_angle;
			double scale_from;
			double scale_to;
			double sin_angle;

			cos_angle = (from.x * to.x) + (from.y * to.y) + (from.z * to.z) + (from.w * to.w);

			if (((how == SlerpType.qshort) && (cos_angle < 0.0)) ||
				((how == SlerpType.qlong) && (cos_angle > 0.0)) ||
				((how == SlerpType.qcw) && (from.w > to.w)) ||
				((how == SlerpType.qccw) && (from.w < to.w)))
			{
				cos_angle = -cos_angle;
				to2.w = -to.w;
				to2.x = -to.x;
				to2.y = -to.y;
				to2.z = -to.z;
			}
			else
				to2 = to;

			if ((1.0 - Math.Abs(cos_angle)) > Util.Epsilon) 
			{
				/* spherical linear interpolation (SLERP) */
				angle = Math.Acos(cos_angle);
				sin_angle  = Math.Sin(angle);
				scale_from = Math.Sin((1.0 - t) * angle) / sin_angle;
				scale_to   = Math.Sin(t         * angle) / sin_angle;
			}
			else {
				/* to prevent divide-by-zero, resort to linear interpolation */
				scale_from = 1.0 - t;
				scale_to   = t;
			}

			q.w = (float)((scale_from * from.w) + (scale_to * to2.w));
			q.x = (float)((scale_from * from.x) + (scale_to * to2.x));
			q.y = (float)((scale_from * from.y) + (scale_to * to2.y));
			q.z = (float)((scale_from * from.z) + (scale_to * to2.z));
			return q;
		}