/// <summary>
/// Creates a JMatrix representing an orientation from a quaternion.
/// </summary>
/// <param name="quaternion">The quaternion the matrix should be created from.</param>
/// <returns>JMatrix representing an orientation.</returns>
#region public static JMatrix CreateFromQuaternion(JQuaternion quaternion)
public static FPMatrix CreateFromLookAt(FPVector3 position, FPVector3 target)
{
FPMatrix result;
LookAt(target - position, FPVector3.up, out result);
return(result);
}
private uint CalculateH(MapNode node, FPVector3 endPos)
{
uint result = 0;
switch (this.m_eHeuristiType)
{
case EHeuristicType.Euclidean:
result = (uint)(endPos - node.Position).sqrMagnitude;
break;
case EHeuristicType.Manhattan:
var x = FPMath.Abs(endPos.x - node.Position.x);
var y = FPMath.Abs(endPos.y - node.Position.y);
var z = FPMath.Abs(endPos.z - node.Position.z);
result = (uint)(x + y + z);
break;
case EHeuristicType.DiagonalManhattan:
var p = endPos - node.Position;
p.x = FPMath.Abs(p.x);
p.y = FPMath.Abs(p.y);
p.z = FPMath.Abs(p.z);
var diag = FPMath.Min(p.x, p.z);
var diag2 = FPMath.Max(p.x, p.z);
result = (uint)((14 * diag / 10) + (diag2 - diag) + p.y);
break;
}
return(result);
}
public static FPMatrix LookAt(FPVector3 forward, FPVector3 upwards)
{
FPMatrix result;
LookAt(forward, upwards, out result);
return(result);
}
public static TSQuaternion FromToRotation(FPVector3 fromVector, FPVector3 toVector)
{
FPVector3 w = FPVector3.Cross(fromVector, toVector);
TSQuaternion q = new TSQuaternion(w.x, w.y, w.z, FPVector3.Dot(fromVector, toVector));
q.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
q.Normalize();
return(q);
}
public static void LookAt(FPVector3 forward, FPVector3 upwards, out FPMatrix result)
{
FPVector3 zaxis = forward; zaxis.Normalize();
FPVector3 xaxis = FPVector3.Cross(upwards, zaxis); xaxis.Normalize();
FPVector3 yaxis = FPVector3.Cross(zaxis, xaxis);
result.M11 = xaxis.x;
result.M21 = yaxis.x;
result.M31 = zaxis.x;
result.M12 = xaxis.y;
result.M22 = yaxis.y;
result.M32 = zaxis.y;
result.M13 = xaxis.z;
result.M23 = yaxis.z;
result.M33 = zaxis.z;
}
public static TSQuaternion AngleAxis(FP angle, FPVector3 axis)
{
axis = axis * FP.Deg2Rad;
axis.Normalize();
FP halfAngle = angle * FP.Deg2Rad * FP.Half;
TSQuaternion rotation;
FP sin = FP.Sin(halfAngle);
rotation.x = axis.x * sin;
rotation.y = axis.y * sin;
rotation.z = axis.z * sin;
rotation.w = FP.Cos(halfAngle);
return(rotation);
}
/// <summary>
/// Creates a matrix which rotates around the given axis by the given angle.
/// </summary>
/// <param name="axis">The axis.</param>
/// <param name="angle">The angle.</param>
/// <param name="result">The resulting rotation matrix</param>
#region public static void CreateFromAxisAngle(ref JVector axis, FP angle, out JMatrix result)
public static void CreateFromAxisAngle(ref FPVector3 axis, FP angle, out FPMatrix result)
{
FP x = axis.x;
FP y = axis.y;
FP z = axis.z;
FP num2 = FP.Sin(angle);
FP num = FP.Cos(angle);
FP num11 = x * x;
FP num10 = y * y;
FP num9 = z * z;
FP num8 = x * y;
FP num7 = x * z;
FP num6 = y * z;
result.M11 = num11 + (num * (FP.One - num11));
result.M12 = (num8 - (num * num8)) + (num2 * z);
result.M13 = (num7 - (num * num7)) - (num2 * y);
result.M21 = (num8 - (num * num8)) - (num2 * z);
result.M22 = num10 + (num * (FP.One - num10));
result.M23 = (num6 - (num * num6)) + (num2 * x);
result.M31 = (num7 - (num * num7)) + (num2 * y);
result.M32 = (num6 - (num * num6)) - (num2 * x);
result.M33 = num9 + (num * (FP.One - num9));
}
public void SetFromToRotation(FPVector3 fromDirection, FPVector3 toDirection)
{
TSQuaternion targetRotation = TSQuaternion.FromToRotation(fromDirection, toDirection);
this.Set(targetRotation.x, targetRotation.y, targetRotation.z, targetRotation.w);
}
public static TSQuaternion Euler(FPVector3 eulerAngles)
{
return(Euler(eulerAngles.x, eulerAngles.y, eulerAngles.z));
}
public static TSQuaternion LookRotation(FPVector3 forward, FPVector3 upwards)
{
return(CreateFromMatrix(FPMatrix.LookAt(forward, upwards)));
}
/// <summary>
/// Creates a matrix which rotates around the given axis by the given angle.
/// </summary>
/// <param name="axis">The axis.</param>
/// <param name="angle">The angle.</param>
/// <returns>The resulting rotation matrix</returns>
public static FPMatrix AngleAxis(FP angle, FPVector3 axis)
{
FPMatrix result; CreateFromAxisAngle(ref axis, angle, out result);
return(result);
}