public void SetRotationY(float rad)
{
double s, c; MathHelpers.SinCos(rad, out s, out c);
M00 = (float)c; M01 = 0; M02 = (float)s;
M10 = 0; M11 = 1; M12 = 0;
M20 = (float)-s; M21 = 0; M22 = (float)c;
}
public void SetRotationVDir(Vec3 vDir, float r)
{
SetRotationVDir(vDir);
double sy, cy; MathHelpers.SinCos(r * 0.5f, out sy, out cy);
var vx = V.X;
var vy = V.Y;
V.X = (float)(vx * cy - V.Z * sy); V.Y = (float)(W * sy + vy * cy); V.Z = (float)(V.Z * cy + vx * sy); W = (float)(W * cy - vy * sy);
}
public void SetRotationXYZ(Vec3 rad)
{
double sx, cx; MathHelpers.SinCos(rad.X, out sx, out cx);
double sy, cy; MathHelpers.SinCos(rad.Y, out sy, out cy);
double sz, cz; MathHelpers.SinCos(rad.Z, out sz, out cz);
double sycz = (sy * cz), sysz = (sy * sz);
M00 = (float)(cy * cz); M01 = (float)(sycz * sx - cx * sz); M02 = (float)(sycz * cx + sx * sz);
M10 = (float)(cy * sz); M11 = (float)(sysz * sx + cx * cz); M12 = (float)(sysz * cx - sx * cz);
M20 = (float)(-sy); M21 = (float)(cy * sx); M22 = (float)(cy * cx);
}
public static Quaternion CreateRotationZ(float radians)
{
var quat = new Quaternion();
float s, c;
MathHelpers.SinCos(radians * 0.5f, out s, out c);
quat._w = c;
quat._v.x = 0;
quat._v.y = 0;
quat._v.z = s;
return(quat);
}
public Quaternion(Angles3 angles)
{
float sx, cx;
MathHelpers.SinCos(angles.x * 0.5f, out sx, out cx);
float sy, cy;
MathHelpers.SinCos(angles.y * 0.5f, out sy, out cy);
float sz, cz;
MathHelpers.SinCos(angles.z * 0.5f, out sz, out cz);
_w = cx * cy * cz + sx * sy * sz;
_v = new Vector3(cz * cy * sx - sz * sy * cx,
cz * sy * cx + sz * cy * sx,
sz * cy * cx - cz * sy * sx);
}
public void SetRotationXYZ(Vec3 angle)
{
float sx;
float cx;
MathHelpers.SinCos((float)(angle.X * (float)0.5), out sx, out cx);
float sy;
float cy;
MathHelpers.SinCos((float)(angle.Y * (float)0.5), out sy, out cy);
float sz;
float cz;
MathHelpers.SinCos((float)(angle.Z * (float)0.5), out sz, out cz);
W = cx * cy * cz + sx * sy * sz;
V.X = cz * cy * sx - sz * sy * cx;
V.Y = cz * sy * cx + sz * cy * sx;
V.Z = sz * cy * cx - cz * sy * sx;
}
/// <summary>
/// Spherical-Interpolation between unit quaternions.
/// </summary>
/// <param name="start">Starting quaternion, will be returned if time is 0</param>
/// <param name="end">End quaternion, will be returned if time is 1</param>
/// <param name="timeRatio">The ratio to slerp between, e.g. 0 = start, 1 = end</param>
/// <returns>Spherically interpolated return value</returns>
public static Quaternion Slerp(Quaternion start, Quaternion end, float timeRatio)
{
var p = start;
var q = end;
var q2 = new Quaternion();
float cosine = p.Dot(q);
if (cosine < 0.0f)
{
cosine = -cosine; q = -q;
} //take shortest arc
if (cosine > 0.9999f)
{
return(Lerp(p, q, timeRatio));
}
q2.w = q.w - p.w * cosine;
q2._v.x = q.v.x - p.v.x * cosine;
q2._v.y = q.v.y - p.v.y * cosine;
q2._v.z = q.v.z - p.v.z * cosine;
float sine = (float)Math.Sqrt(q2.Dot(q2));
float s, c;
MathHelpers.SinCos((float)Math.Atan2(sine, cosine) * timeRatio, out s, out c);
var returnValue = new Quaternion();
returnValue.w = p.w * c + q2.w * s / sine;
returnValue._v.x = p.v.x * c + q2.v.x * s / sine;
returnValue._v.y = p.v.y * c + q2.v.y * s / sine;
returnValue._v.z = p.v.z * c + q2.v.z * s / sine;
return(returnValue);
}
public void SetRotationX(float r)
{
float s, c;
MathHelpers.SinCos((float)(r * (float)0.5), out s, out c); W = c; V.X = s; V.Y = 0; V.Z = 0;
}
