martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Apply random rotation on test bodies when resetting scene.

master
Martin Felis 2020-11-20 22:54:44 +01:00
parent 17a8a0347f
commit 8dd349752d
1 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/simulator.cc
View File

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