diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp index dead01c550f6bc9f95689338d80a91914fadf38d..bc60514c6197acbf7be1ddcc86b99c3cc0e7de7f 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletRobot.cpp @@ -1100,6 +1100,8 @@ namespace SimDynamics return (a2 - link.jointValueOffset); // inverted joint direction in bullet #else boost::shared_ptr<btHingeConstraint> hinge = boost::dynamic_pointer_cast<btHingeConstraint>(link.joint); + if (!hinge) + return 0.0f; /*if (!hinge) { @@ -1487,7 +1489,7 @@ namespace SimDynamics // parent is at correct pose, we can update *it if (parent) { - //(*it)->updatePose(false); + (*it)->updatePose(false); } // if root, we also have to delete node from list diff --git a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp index 51b9d44e72c957eb8be733af42eb83f52569637a..1db11f7d2ae32d6d35c3e5e0350aa06692fba821 100644 --- a/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp +++ b/VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.cpp @@ -1843,6 +1843,10 @@ namespace VirtualRobot SoSeparator* CoinVisualizationFactory::CreateArrow(const Eigen::Vector3f& n, float length, float width, const Color& color) { + Eigen::Vector3f n2 = n; + if (n2.norm()<1e-10) + n2 << 0,0,1; + n2.normalize(); float coneHeight = width * 6.0f; float coneBotomRadius = width * 2.5f; SoSeparator* res = new SoSeparator; @@ -1851,12 +1855,12 @@ namespace VirtualRobot u->units = SoUnits::MILLIMETERS; res->addChild(u); - SbVec3f objNormal(n(0), n(1), n(2)); + SbVec3f objNormal(n2(0), n2(1), n2(2)); SbMatrix objNormalTrafo; objNormalTrafo.makeIdentity(); SbRotation objNormalRot(SbVec3f(0, 1.0f, 0), objNormal); - // get rif of warnings when angle==0 + // get rid of warnings when angle==0 SbVec3f axis; float angle; objNormalRot.getValue(axis, angle);