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)); }