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