public override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-13f, 6, -13f); camera.Yaw = MathF.PI * 3f / 4; camera.Pitch = MathF.PI * 0.05f; Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0))); { var shapeA = new Cylinder(0.5f, 1f); var poseA = new RigidPose(new Vector3(0, 0, 0)); var shapeB = new Cylinder(1f, 2f); //var positionB = new Vector3(-0.2570486f, 1.780561f, -1.033215f); //var localOrientationBMatrix = new Matrix3x3 //{ // X = new Vector3(0.9756086f, 0.1946615f, 0.101463f), // Y = new Vector3(-0.1539477f, 0.9362175f, -0.3159063f), // Z = new Vector3(-0.1564862f, 0.2925809f, 0.9433496f) //}; //var positionB = new Vector3(-1.437585f, 0.386236f, -1.124907f); var positionB = new Vector3(-0.437585f, 0.386236f, -.124907f); var localOrientationBMatrix = new Matrix3x3 { X = new Vector3(-0.7615921f, 0.001486331f, -0.648055f), Y = new Vector3(0.6341797f, 0.2075436f, -0.7448099f), Z = new Vector3(-0.1333926f, -0.9782246f, -0.1590062f) }; //var poseB = new RigidPose(new Vector3(-0.2570486f, 1.780561f, -1.033215f), Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 1, 1)), MathF.PI * 0.35f)); var poseB = new RigidPose(positionB, Quaternion.CreateFromRotationMatrix(localOrientationBMatrix)); basePosition = default; shapeLines = MinkowskiShapeVisualizer.CreateLines <Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>( shapeA, shapeB, poseA, poseB, 65536, 0.01f, new Vector3(0.4f, 0.4f, 0), 0.1f, new Vector3(0, 1, 0), default, basePosition, BufferPool);
public void Test(Random random, int innerIterations) { Quaternion q; q.X = (float)random.NextDouble() * 2 - 1; q.Y = (float)random.NextDouble() * 2 - 1; q.Z = (float)random.NextDouble() * 2 - 1; q.W = (float)random.NextDouble() * 2 - 1; Quaternion.NormalizeRef(ref q); for (int i = 0; i < innerIterations; ++i) { Matrix3x3.CreateFromQuaternion(q, out var r); Quaternion.CreateFromRotationMatrix(r, out var qTest); #if DEBUG const float epsilon = 1e-6f; var lengthX = r.X.Length(); var lengthY = r.Y.Length(); var lengthZ = r.Z.Length(); Debug.Assert( Math.Abs(1 - lengthX) < epsilon && Math.Abs(1 - lengthY) < epsilon && Math.Abs(1 - lengthZ) < epsilon); if (qTest.X * q.X < 0) { Quaternion.Negate(qTest, out qTest); } Debug.Assert( Math.Abs(qTest.X - q.X) < epsilon && Math.Abs(qTest.Y - q.Y) < epsilon && Math.Abs(qTest.Z - q.Z) < epsilon && Math.Abs(qTest.W - q.W) < epsilon); #endif } }