using setFrc in RigidBody::calcForcesAndTorques will discard if there are force and torque applied in rigid body itself. use addFrc instead.