public Vector3 getInvNormal()
{
if (!HaveStep)
{
return(new Vector3(0, 0, 0));
}
/*
* Original code
* btMatrix3x3 frame;
* // Build it as step to world
* frame[2].setValue(0, 0, 1);
* frame[1] = mStepNormal;
* frame[0] = frame[1].cross(frame[2]);
* frame[0].normalize();
* // Convert it to world to step
* frame = frame.transpose();
* return frame[1];
*/
Matrix frame = Matrix.Zero;
// Build it as step to world
frame.Column3 = new Vector4(0, 0, 1, 0);
frame.Column2 = new Vector4(_stepNormal, 0);
var crossed = Vector3.Cross(
new Vector3(frame.Column2.X, frame.Column2.Y, frame.Column2.Z),
new Vector3(frame.Column3.X, frame.Column3.Y, frame.Column3.Z));
frame.Column1 = new Vector4(crossed.X, crossed.Y, crossed.Z, 0);
frame.Column1.Normalize();
// Convert it to world to step
frame.Transpose();
return(new Vector3(frame.Column2.X, frame.Column2.Y, frame.Column2.Z));
}
