public Bone(BinaryReader reader) { Name = reader.ReadString(); parentName = reader.ReadString(); InheritScale = reader.ReadBoolean(); InheritRotation = reader.ReadBoolean(); Position = reader.ReadVector3(); RotationMatrix = reader.ReadMatrix3(); Rotation = Quaternion.FromMatrix3(RotationMatrix); }
public static void Conjugate(ref Quaternion quaternoin, out Quaternion result) { result.X = -quaternoin.X; result.Y = -quaternoin.Y; result.Z = -quaternoin.Z; result.W = quaternoin.W; }
public static void Transform(ref Vector3 vector, ref Quaternion quaternion, out Vector3 result) { float x2 = quaternion.X + quaternion.X; float y2 = quaternion.Y + quaternion.Y; float z2 = quaternion.Z + quaternion.Z; float xx2 = quaternion.X * x2; float xy2 = quaternion.X * y2; float xz2 = quaternion.X * z2; float yy2 = quaternion.Y * y2; float yz2 = quaternion.Y * z2; float zz2 = quaternion.Z * z2; float wx2 = quaternion.W * x2; float wy2 = quaternion.W * y2; float wz2 = quaternion.W * z2; result.X = vector.X * ((1f - yy2) - zz2) + vector.Y * (xy2 - wz2) + vector.Z * (xz2 + wy2); result.Y = vector.X * (xy2 + wz2) + vector.Y * ((1f - xx2) - zz2) + vector.Z * (yz2 - wx2); result.Z = vector.X * (xz2 - wy2) + vector.Y * (yz2 + wx2) + vector.Z * ((1f - xx2) - yy2); }
public RigidTransform3(Vector3 position) { Position = position; Orientation = Quaternion.Identity; }
public Quaternion Multiply(Quaternion quaternion) { return new Quaternion ( W*quaternion.X + X*quaternion.W + Y*quaternion.Z - Z*quaternion.Y, W*quaternion.Y - X*quaternion.Z + Y*quaternion.W + Z*quaternion.X, W*quaternion.Z + X*quaternion.Y - Y*quaternion.X + Z*quaternion.W, W*quaternion.W - X*quaternion.X - Y*quaternion.Y - Z*quaternion.Z ); }
public static void Sub(float value1, ref Quaternion value2, out Quaternion result) { result.X = value1 - value2.X; result.Y = value1 - value2.Y; result.Z = value1 - value2.Z; result.W = value1 - value2.W; }
// - public static void Sub(ref Quaternion value1, ref Quaternion value2, out Quaternion result) { result.X = value1.X - value2.X; result.Y = value1.Y - value2.Y; result.Z = value1.Z - value2.Z; result.W = value1.W - value2.W; }
public static void Slerp(ref Quaternion start, ref Quaternion end, float interpolationAmount, out Quaternion result) { float cosHalfTheta = start.W * end.W + start.X * end.X + start.Y * end.Y + start.Z * end.Z; if (cosHalfTheta < 0) { //Negating a quaternion results in the same orientation, but we need cosHalfTheta to be positive to get the shortest path. end = -end; cosHalfTheta = -cosHalfTheta; } // If the orientations are similar enough, then just pick one of the inputs. if (cosHalfTheta > .999999f) { result = start; return; } // Calculate temporary values. float halfTheta = (float)Math.Acos(cosHalfTheta); float sinHalfTheta = (float)Math.Sqrt(1.0 - cosHalfTheta * cosHalfTheta); //Check to see if we're 180 degrees away from the target. if (Math.Abs(sinHalfTheta) < 0.00001f) { result = (start + end) * .5f; return; } //Blend the two quaternions to get the result! float aFraction = (float)Math.Sin((1 - interpolationAmount) * halfTheta) / sinHalfTheta; float bFraction = (float)Math.Sin(interpolationAmount * halfTheta) / sinHalfTheta; result = start * aFraction + end * bFraction; }
public static void FromRotationAxis(float axisX, float axisY, float axisZ, float angle, out Quaternion result) { angle *= .5f; var sin = (float)Math.Sin(angle); result.X = axisX * sin; result.Y = axisY * sin; result.Z = axisZ * sin; result.W = (float)Math.Cos(angle); }
public static void FromRotationAxis(ref Vector3 axis, float angle, out Quaternion result) { angle *= .5f; var sin = (float)Math.Sin(angle); result.X = axis.X * sin; result.Y = axis.Y * sin; result.Z = axis.Z * sin; result.W = (float)Math.Cos(angle); }
public static void FromMatrix4(ref Matrix4 matrix, out Quaternion result) { float w = (float)Math.Sqrt(1 + matrix.X.X + matrix.Y.Y + matrix.Z.Z) * .5f; float delta = 1 / (w * 4); result.X = (matrix.Z.Y - matrix.Y.Z) * delta; result.Y = (matrix.X.Z - matrix.Z.X) * delta; result.Z = (matrix.Y.X - matrix.X.Y) * delta; result.W = w; }
public static void FromEuler(float eulerX, float eulerY, float eulerZ, out Quaternion result) { eulerX *= .5f; eulerY *= .5f; eulerZ *= .5f; float c1 = (float)Math.Cos(eulerY); float s1 = (float)Math.Sin(eulerY); float c2 = (float)Math.Cos(eulerZ); float s2 = (float)Math.Sin(eulerZ); float c3 = (float)Math.Cos(eulerX); float s3 = (float)Math.Sin(eulerX); float c1c2 = c1*c2; float s1s2 = s1*s2; float s1c2 = s1*c2; float c1s2 = c1*s2; result.W = c1c2*c3 - s1s2*s3; result.X = c1c2*s3 + s1s2*c3; result.Y = s1c2*c3 + c1s2*s3; result.Z = c1s2*c3 - s1c2*s3; }
public static void FromEuler(Vector3 euler, out Quaternion result) { euler.X *= .5f; euler.Y *= .5f; euler.Z *= .5f; float c1 = (float)Math.Cos(euler.Y); float s1 = (float)Math.Sin(euler.Y); float c2 = (float)Math.Cos(euler.Z); float s2 = (float)Math.Sin(euler.Z); float c3 = (float)Math.Cos(euler.X); float s3 = (float)Math.Sin(euler.X); float c1c2 = c1*c2; float s1s2 = s1*s2; float s1c2 = s1*c2; float c1s2 = c1*s2; result.W = c1c2*c3 - s1s2*s3; result.X = c1c2*s3 + s1s2*c3; result.Y = s1c2*c3 + c1s2*s3; result.Z = c1s2*c3 - s1c2*s3; }
public static void Div(ref Quaternion value1, float value2, out Quaternion result) { result.X = value1.X / value2; result.Y = value1.Y / value2; result.Z = value1.Z / value2; result.W = value1.W / value2; }
// / public static void Div(ref Quaternion value1, ref Quaternion value2, out Quaternion result) { result.X = value1.X / value2.X; result.Y = value1.Y / value2.Y; result.Z = value1.Z / value2.Z; result.W = value1.W / value2.W; }
public static void NormalizeSafe(ref Quaternion quaternion, out Quaternion result, out float length) { float dis = (float)Math.Sqrt((quaternion.X*quaternion.X) + (quaternion.Y*quaternion.Y) + (quaternion.Z*quaternion.Z) + (quaternion.W*quaternion.W)); length = dis; if (dis == 0) result = new Quaternion(); else result = quaternion * (1/dis); }
public static void RotationAxis(ref Quaternion quaternion, out Vector3 axis, out float angle) { angle = (float)Math.Acos(quaternion.W) * MathUtilities.Pi2; float sinAngle = (float)Math.Sqrt(1 - (quaternion.W*quaternion.W)); if (sinAngle == 0) sinAngle = 1; sinAngle = 1 / sinAngle; axis = new Vector3(quaternion.X*sinAngle, quaternion.Y*sinAngle, quaternion.Z*sinAngle); }
public static void FromSphericalRotation(float latitude, float longitude, float angle, out Quaternion result) { angle *= .5f; float sa = (float)Math.Sin(angle); float cLat = (float)Math.Cos(latitude); float sLat = (float)Math.Sin(latitude); float cLong = (float)Math.Cos(longitude); float sLong = (float)Math.Sin(longitude); result.X = sa*cLat*sLong; result.Y = sa*sLat; result.Z = sa*sLat*cLong; result.W = (float)Math.Cos(angle); }
public static void SphericalRotation(ref Quaternion quaternion, out float latitude, out float longitude, out float angle) { angle = (float)Math.Acos(quaternion.W) * MathUtilities.Pi2; float sinAngle = (float)Math.Sqrt(1 - (quaternion.W*quaternion.W)); if (sinAngle == 0) sinAngle = 1; sinAngle = 1 / sinAngle; float x = quaternion.X * sinAngle; float y = quaternion.Y * sinAngle; float z = quaternion.Z * sinAngle; latitude = -(float)Math.Asin(y); if ((x*x) + (z*z) == 0) longitude = 0; else longitude = (float)Math.Atan2(x, z) * MathUtilities.Pi; if (longitude < 0) longitude += MathUtilities.Pi2; }
public static void Length(ref Quaternion quaternion, out float result) { result = (float)Math.Sqrt((quaternion.X*quaternion.X) + (quaternion.Y*quaternion.Y) + (quaternion.Z*quaternion.Z) + (quaternion.W*quaternion.W)); }
public static void Sub(ref Quaternion value1, float value2, out Quaternion result) { result.X = value1.X - value2; result.Y = value1.Y - value2; result.Z = value1.Z - value2; result.W = value1.W - value2; }
// * public static void Mul(ref Quaternion value1, ref Quaternion value2, out Quaternion result) { result.X = value1.X * value2.X; result.Y = value1.Y * value2.Y; result.Z = value1.Z * value2.Z; result.W = value1.W * value2.W; }
public Quaternion Concatenate(Quaternion quaternion) { return new Quaternion ( quaternion.W*X + quaternion.X*W + quaternion.Y*Z - quaternion.Z*Y, quaternion.W*Y - quaternion.X*Z + quaternion.Y*W + quaternion.Z*X, quaternion.W*Z + quaternion.X*Y - quaternion.Y*X + quaternion.Z*W, quaternion.W*W - quaternion.X*X - quaternion.Y*Y - quaternion.Z*Z ); }
public static void Mul(ref Quaternion value1, float value2, out Quaternion result) { result.X = value1.X * value2; result.Y = value1.Y * value2; result.Z = value1.Z * value2; result.W = value1.W * value2; }
public RigidTransform3(Quaternion orienation, Vector3 position) { Orientation = orienation; Position = position; }
public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result) { result.X = quaternion1.W*quaternion2.X + quaternion1.X*quaternion2.W + quaternion1.Y*quaternion2.Z - quaternion1.Z*quaternion2.Y; result.Y = quaternion1.W*quaternion2.Y - quaternion1.X*quaternion2.Z + quaternion1.Y*quaternion2.W + quaternion1.Z*quaternion2.X; result.Z = quaternion1.W*quaternion2.Z + quaternion1.X*quaternion2.Y - quaternion1.Y*quaternion2.X + quaternion1.Z*quaternion2.W; result.W = quaternion1.W*quaternion2.W - quaternion1.X*quaternion2.X - quaternion1.Y*quaternion2.Y - quaternion1.Z*quaternion2.Z; }
public RigidTransform3(Quaternion orienation) { Position = new Vector3(); Orientation = orienation; }
public static void Neg(ref Quaternion value, out Quaternion result) { result.X = -value.X; result.Y = -value.Y; result.Z = -value.Z; result.W = -value.W; }
public Vector3 Transform(Quaternion quaternion) { float x2 = quaternion.X + quaternion.X; float y2 = quaternion.Y + quaternion.Y; float z2 = quaternion.Z + quaternion.Z; float xx2 = quaternion.X * x2; float xy2 = quaternion.X * y2; float xz2 = quaternion.X * z2; float yy2 = quaternion.Y * y2; float yz2 = quaternion.Y * z2; float zz2 = quaternion.Z * z2; float wx2 = quaternion.W * x2; float wy2 = quaternion.W * y2; float wz2 = quaternion.W * z2; return new Vector3 ( X * ((1f - yy2) - zz2) + Y * (xy2 - wz2) + Z * (xz2 + wy2), X * (xy2 + wz2) + Y * ((1f - xx2) - zz2) + Z * (yz2 - wx2), X * (xz2 - wy2) + Y * (yz2 + wx2) + Z * ((1f - xx2) - yy2) ); }
public static void Normalize(ref Quaternion quaternion, out Quaternion result) { result = quaternion * (1 / (float)Math.Sqrt((quaternion.X*quaternion.X) + (quaternion.Y*quaternion.Y) + (quaternion.Z*quaternion.Z) + (quaternion.W*quaternion.W))); }