public static aVec3 operator *(aQuat lhs, aVec3 rhs)
{
var vector = new aVec3(lhs.x, lhs.y, lhs.z);
var cross = (aVec3.CrossProduct(vector, rhs) * 2);
return(rhs + (cross * lhs.w) + aVec3.CrossProduct(vector, cross));
}
public static aQuat Slerp(aQuat start, aQuat end, float percent)
{
var cos = aQuat.DotProduct(start, end);
if (cos < 0.0f)
{
cos = -cos;
end = -end;
}
if ((1.0f - cos) > Single.Epsilon)
{
var angle = (float)SystemMath.Acos(cos);
var vector = new aVec3(
(float)SystemMath.Sin(angle * (1.0f - percent)),
(float)SystemMath.Sin(angle * percent),
(float)SystemMath.Sin(angle)
);
return(new aQuat(
(start.x * vector.x + end.x * vector.y) / vector.z,
(start.y * vector.x + end.y * vector.y) / vector.z,
(start.z * vector.x + end.z * vector.y) / vector.z,
(start.w * vector.x + end.w * vector.y) / vector.z
));
}
return(aQuat.Lerp(start, end, percent));
}
public static aVec3 CrossProduct(aVec3 first, aVec3 second)
{
return(new aVec3(
first.y * second.z - first.z * second.y,
first.z * second.x - first.x * second.z,
first.x * second.y - first.y * second.x
));
}
public static aQuat FromForwardUpward(aVec3 forward, aVec3 up)
{
var vector = (forward + up).Normalized;
return(new aQuat(
up.y * vector.z - up.z * vector.y,
up.z * vector.x - up.x * vector.z,
up.x * vector.y - up.y * vector.x,
aVec3.DotProduct(up, vector)
));
}
public static aQuat FromAxisAngle(aVec3 axis, float angle)
{
var vector = axis * (float)SystemMath.Sin(angle * 0.5f);
return(new aQuat(
vector.x,
vector.y,
vector.z,
(float)SystemMath.Cos(angle * 0.5f)
));
}
public static aQuat FromLookAt(aVec3 src, aVec3 dest)
{
var forward = (dest - src).Normalized;
var dot = aVec3.DotProduct(aVec3.UnitZ, forward);
if (SystemMath.Abs(dot + 1.0f) < Single.Epsilon)
{
return(new aQuat(aVec3.UnitY.x, aVec3.UnitY.y, aVec3.UnitY.z, (float)SystemMath.PI));
}
else if (SystemMath.Abs(dot - 1.0f) < Single.Epsilon)
{
return(aQuat.Identity);
}
var axis = aVec3.CrossProduct(aVec3.UnitZ, forward).Normalized;
var angle = (float)SystemMath.Acos(dot);
return(FromAxisAngle(axis, angle));
}
public static aVec3 Lerp(aVec3 start, aVec3 end, float percentage)
{
return (start * (1.0f - percentage)) + (end * percentage);
}
public static float DotProduct(aVec3 first, aVec3 second)
{
return (first.x * second.x + first.y * second.y + first.z * second.z);
}
public static float Distance(aVec3 from, aVec3 to)
{
return (from - to).Magnitude;
}
public static aVec3 CrossProduct(aVec3 first, aVec3 second)
{
return new aVec3(
first.y * second.z - first.z * second.y,
first.z * second.x - first.x * second.z,
first.x * second.y - first.y * second.x
);
}
public static aQuat Slerp(aQuat start, aQuat end, float percent)
{
var cos = aQuat.DotProduct(start, end);
if (cos < 0.0f) {
cos = -cos;
end = -end;
}
if ((1.0f - cos) > Single.Epsilon) {
var angle = (float)SystemMath.Acos(cos);
var vector = new aVec3(
(float)SystemMath.Sin(angle * (1.0f - percent)),
(float)SystemMath.Sin(angle * percent),
(float)SystemMath.Sin(angle)
);
return new aQuat(
(start.x * vector.x + end.x * vector.y) / vector.z,
(start.y * vector.x + end.y * vector.y) / vector.z,
(start.z * vector.x + end.z * vector.y) / vector.z,
(start.w * vector.x + end.w * vector.y) / vector.z
);
}
return aQuat.Lerp(start, end, percent);
}
public static aQuat FromEulerAngles(aVec3 euler)
{
return FromEulerAngles(euler.x, euler.y, euler.z);
}
public static aQuat FromAxisAngle(aVec3 axis, float angle)
{
var vector = axis * (float)SystemMath.Sin(angle * 0.5f);
return new aQuat(
vector.x,
vector.y,
vector.z,
(float)SystemMath.Cos(angle * 0.5f)
);
}
public static aVec3 operator *(aQuat lhs, aVec3 rhs)
{
var vector = new aVec3(lhs.x, lhs.y, lhs.z);
var cross = (aVec3.CrossProduct(vector, rhs) * 2);
return rhs + (cross * lhs.w) + aVec3.CrossProduct(vector, cross);
}
public static aQuat FromEulerAngles(aVec3 euler)
{
return(FromEulerAngles(euler.x, euler.y, euler.z));
}
public static float DotProduct(aVec3 first, aVec3 second)
{
return(first.x * second.x + first.y * second.y + first.z * second.z);
}
public static aVec3 Lerp(aVec3 start, aVec3 end, float percentage)
{
return((start * (1.0f - percentage)) + (end * percentage));
}
public static aQuat FromForwardUpward(aVec3 forward, aVec3 up)
{
var vector = (forward + up).Normalized;
return new aQuat(
up.y * vector.z - up.z * vector.y,
up.z * vector.x - up.x * vector.z,
up.x * vector.y - up.y * vector.x,
aVec3.DotProduct(up, vector)
);
}
public static float Distance(aVec3 from, aVec3 to)
{
return((from - to).Magnitude);
}
public static aQuat FromLookAt(aVec3 src, aVec3 dest)
{
var forward = (dest - src).Normalized;
var dot = aVec3.DotProduct(aVec3.UnitZ, forward);
if (SystemMath.Abs(dot + 1.0f) < Single.Epsilon) {
return new aQuat(aVec3.UnitY.x, aVec3.UnitY.y, aVec3.UnitY.z, (float)SystemMath.PI);
}
else if (SystemMath.Abs(dot - 1.0f) < Single.Epsilon) {
return aQuat.Identity;
}
var axis = aVec3.CrossProduct(aVec3.UnitZ, forward).Normalized;
var angle = (float)SystemMath.Acos(dot);
return FromAxisAngle(axis, angle);
}
