static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger, bool lastThree = false)
{
vec3.setZero();
btArray<std::string> pieces;
btArray<float> rgba;
urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" "));
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
if (rgba.size() < 3)
{
logger->reportWarning("Couldn't parse vector3");
return false;
}
if (lastThree) {
vec3.setValue(rgba[rgba.size()-3], rgba[rgba.size()-2], rgba[rgba.size()-1]);
}
else
{
vec3.setValue(rgba[0],rgba[1],rgba[2]);
}
return true;
}
void SoftBodyBullet::get_first_node_origin(btVector3 &p_out_origin) const {
if (bt_soft_body && bt_soft_body->m_nodes.size()) {
p_out_origin = bt_soft_body->m_nodes[0].m_x;
} else {
p_out_origin.setZero();
}
}
void ComputeController(btVector3 ¤tSpeed, const btVector3 &delta, const btVector3 &maxSpeed, float scaleDelta, float damping) {
// Timestep scale
btVector3 acceleration = delta * scaleDelta;
if (currentSpeed.fuzzyZero() && !currentSpeed.isZero()) {
currentSpeed.setZero();
}
acceleration += currentSpeed * -damping;
// Clamp the acceleration to max speed
for(int i = 2; i >= 0; i--) {
if (fabs(acceleration[i]) < maxSpeed[i]) continue;
acceleration[i] = (acceleration[i] < 0) ? -maxSpeed[i] : maxSpeed[i];
}
currentSpeed += acceleration;
}
void btRayShape::getPreferredPenetrationDirection(int index, btVector3 &penetrationVector) const {
penetrationVector.setZero();
}
void btRayShape::calculateLocalInertia(btScalar mass, btVector3 &inertia) const {
inertia.setZero();
}