From 8dd349752df3ef0c3ffbe042b3d43bcd6a1cf0f6 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Fri, 20 Nov 2020 22:54:44 +0100
Subject: [PATCH] Apply random rotation on test bodies when resetting scene.

---
 src/simulator.cc | 16 ++++++++++++++--
 1 file changed, 14 insertions(+), 2 deletions(-)

diff --git a/src/simulator.cc b/src/simulator.cc
index 40f263c..a77216e 100644
--- a/src/simulator.cc
+++ b/src/simulator.cc
@@ -53,7 +53,15 @@ void simulator_init() {
         restitution,
         Vector3d::Random() * 5.,
         Vector3d::Zero());
-    sWorld.mBodies.push_back(sphere_body);
+
+//    SimBody body = CreateSphereBody(
+//        1.,
+//        1.,
+//        restitution,
+//        Vector3d::Random() * 5.,
+//        Vector3d::Zero());
+
+    sWorld.mBodies.push_back(body);
   }
 
   for (int i = 0; i < sWorld.mBodies.size(); i++) {
@@ -80,7 +88,8 @@ void simulator_reset() {
     for (int i = 0; i < body.mModel.mBodies.size(); i++) {
       if (body.mModel.mJoints[i].mJointType
           == RigidBodyDynamics::JointTypeSpherical) {
-        body.mModel.SetQuaternion(i, Quaternion(0., 0., 0., 1.), body.q);
+        Quaternion orientation = Quaternion::Random().normalize();
+        body.mModel.SetQuaternion(i, orientation, body.q);
       }
     }
   }
@@ -158,6 +167,9 @@ void simulator_update(double dt) {
     ImGui::DragFloat3("Vel", body_vel.data(), 0.01f, -20.0f, 20.0f);
     body.qdot.block(0, 0, 3, 1) = body_vel;
 
+    Vector4f body_rot = body.mModel.GetQuaternion(2, body.q);
+    ImGui::DragFloat4("Rot", body_rot.data(), 0.01f, -20.0f, 20.0f);
+
     body.updateCollisionShapes();
   }