public bool CreateFrame(UnityEngine.Vector3 forward, UnityEngine.Vector3 up, UnityEngine.Vector3 constraintPoint, ref BulletSharp.Math.Matrix m, ref string errorMsg)
{
BulletSharp.Math.Vector4 x;
BulletSharp.Math.Vector4 y;
BulletSharp.Math.Vector4 z;
if (forward == Vector3.zero)
{
errorMsg = "forward vector must not be zero";
return false;
}
forward.Normalize();
if (up == Vector3.zero)
{
errorMsg = "up vector must not be zero";
return false;
}
Vector3 right = Vector3.Cross(forward, up);
if (right == Vector3.zero)
{
errorMsg = "forward and up vector must not be colinear";
return false;
}
up = Vector3.Cross(right, forward);
right.Normalize();
up.Normalize();
x.X = forward.x; x.Y = forward.y; x.Z = forward.z; x.W = 0f;
y.X = up.x; y.Y = up.y; y.Z = up.z; y.W = 0f;
z.X = right.x; z.Y = right.y; z.Z = right.z; z.W = 0f;
m.Row1 = x;
m.Row2 = y;
m.Row3 = z;
m.Origin = constraintPoint.ToBullet();
return true;
}
