public void CopyFrom(Quaternion4F q) { W = q.W; X = q.X; Y = q.Y; Z = q.Z; }
public void FromQuaternion(Quaternion4F q) { SetIdentity(); // Compute a few values to optimize common subexpressions var ww = 2.0f * q.W; var xx = 2.0f * q.X; var yy = 2.0f * q.Y; var zz = 2.0f * q.Z; // Set the matrix elements. There is still a little more // opportunity for optimization due to the many common // subexpressions. We'll let the compiler handle that... Values[0] = 1.0f - (yy * q.Y) - (zz * q.Z); Values[1] = (xx * q.Y) + (ww * q.Z); Values[2] = (xx * q.Z) - (ww * q.Y); Values[4] = (xx * q.Y) - (ww * q.Z); Values[5] = 1.0f - (xx * q.X) - (zz * q.Z); Values[6] = (yy * q.Z) + (ww * q.X); Values[8] = (xx * q.Z) + (ww * q.Y); Values[9] = (yy * q.Z) - (ww * q.X); Values[10] = 1.0f - (xx * q.X) - (yy * q.Y); }
/// <summary> /// Do Spherical linear interpolation between two quaternions /// </summary> /// <param name="q1">The first quaternion</param> /// <param name="q2">The second quaternion</param> /// <param name="blend">The blend factor</param> /// <returns>A smooth blend between the given quaternions</returns> public static Quaternion4F OtkSlerp(Quaternion4F q1, Quaternion4F q2, float blend) { // if either input is zero, return the other. if (q1.LengthSquared == 0.0f) { if (q2.LengthSquared == 0.0f) { return(Identity); } return(q2); } if (q2.LengthSquared == 0.0f) { return(q1); } var cosHalfAngle = q1.W * q2.W + q1.Xyz.Dot(q2.Xyz); if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) { // angle = 0.0f, so just return one input. return(q1); } if (cosHalfAngle < 0.0f) { q2.Xyz.Negate(); q2.W = -q2.W; cosHalfAngle = -cosHalfAngle; } float blendA; float blendB; if (cosHalfAngle < 0.99f) { // do proper slerp for big angles var halfAngle = (float)Math.Acos(cosHalfAngle); var sinHalfAngle = (float)Math.Sin(halfAngle); var oneOverSinHalfAngle = 1.0f / sinHalfAngle; blendA = (float)Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle; blendB = (float)Math.Sin(halfAngle * blend) * oneOverSinHalfAngle; } else { // do lerp if angle is really small. blendA = 1.0f - blend; blendB = blend; } var result = new Quaternion4F(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W); if (result.LengthSquared > 0.0f) { result.Normalize(); return(result); } return(Identity); }
/// <summary> /// Devuelve un nuevo Quaternion que representa la concatenación de rotaciones de ambos. /// </summary> /// <param name="tempLeft"></param> /// <param name="tempRight"></param> /// <returns></returns> public static Quaternion4F operator *(Quaternion4F left, Quaternion4F right) { var q = new Quaternion4F(); q.X = left.W * right.X + left.X * right.W + left.Y * right.Z - left.Z * right.Y; q.Y = left.W * right.Y + left.Y * right.W + left.Z * right.X - left.X * right.Z; q.Z = left.W * right.Z + left.Z * right.W + left.X * right.Y - left.Y * right.X; q.W = left.W * right.W - left.X * right.X - left.Y * right.Y - left.Z * right.Z; return(q); }
/// <summary> /// this = this * q /// </summary> /// <param name="q"></param> public void Multiply(Quaternion4F q) { var auxX = _w * q._x + _x * q._w + _y * q._z - _z * q._y; var auxY = _w * q._y + _y * q._w + _z * q._x - _x * q._z; var auxZ = _w * q._z + _z * q._w + _x * q._y - _y * q._x; var auxW = _w * q._w - _x * q._x - _y * q._y - _z * q._z; _x = auxX; _y = auxY; _z = auxZ; _w = auxW; }
public static Quaternion4F FromRadiansAxis(float radianes, float x, float y, float z) { var quaternion = new Quaternion4F(); var num2 = radianes * 0.5f; var num = (float)Math.Sin(num2); var num3 = (float)Math.Cos(num2); quaternion._x = x * num; quaternion._y = y * num; quaternion._z = z * num; quaternion._w = num3; quaternion.Normalize(); return(quaternion); }
/// <summary> /// Multiplica esta matriz (this) por la representada por el quaternion pasado como parámetro. /// <para>this = this * Quaternion</para> /// </summary> /// <param name="qRot"></param> public virtual void Rotate(Quaternion4F qRot) { //qRot.GetMatriz4(ref auxMat4); AuxMat4.FromQuaternion(qRot); MultipliesBy(AuxMat4); }
public static Quaternion4F Slerp(Quaternion4F q0, Quaternion4F q1, float t) { var qresult = new Quaternion4F(); // Check for out-of range parameter and return edge points if so if (t <= 0.0) { qresult.CopyFrom(q0); return(qresult); } if (t >= 1.0) { qresult.CopyFrom(q1); return(qresult); } // Compute "cosine of angle between quaternions" using dot product var cosOmega = Dot(q0, q1); // If negative dot, use -q1. Two quaternions q and -q // represent the same rotation, but may produce // different slerp. We chose q or -q to rotate using // the acute angle. var q1W = q1._w; var q1X = q1._x; var q1Y = q1._y; var q1Z = q1._z; if (cosOmega < 0.0) { q1W = -q1W; q1X = -q1X; q1Y = -q1Y; q1Z = -q1Z; cosOmega = -cosOmega; } // We should have two unit quaternions, so dot should be <= 1.0 if (!(cosOmega < 1.1f)) { throw new Exception("Error en Quaternion.Slerp(), cosOmega > 1.1f"); } // Compute interpolation fraction, checking for quaternions // almost exactly the same float k0, k1; if (cosOmega > 0.9999) { // Very close - just use linear interpolation, // which will protect againt a divide by zero k0 = 1.0f - t; k1 = t; } else { // Compute the sin of the angle using the // trig identity sin^2(omega) + cos^2(omega) = 1 var sinOmega = (float)Math.Sqrt(1.0 - (cosOmega * cosOmega)); // Compute the angle from its sin and cosine var omega = (float)Math.Atan2(sinOmega, cosOmega); // Compute inverse of denominator, so we only have // to divide once var oneOverSinOmega = 1.0f / sinOmega; // Compute interpolation parameters k0 = (float)(Math.Sin((1.0 - t) * omega) * oneOverSinOmega); k1 = (float)(Math.Sin(t * omega) * oneOverSinOmega); } // Interpolate and return new quaternion return(new Quaternion4F( (k0 * q0._w) + (k1 * q1W), (k0 * q0._x) + (k1 * q1X), (k0 * q0._y) + (k1 * q1Y), (k0 * q0._z) + (k1 * q1Z) )); }
public static float Dot(Quaternion4F a, Quaternion4F b) { return((a._w * b._w) + (a._x * b._x) + (a._y * b._y) + (a._z * b._z)); }
public float Dot(Quaternion4F quat) { return(_w * quat._w + _x * quat._x + _y * quat._y + _z * quat._z); }