char DynamicsSolver::selectByRayHit(const Vector3F & origin, const Vector3F & ray, Vector3F & hitP)
{
m_activeBody = 0;
btVector3 fromP(origin.x, origin.y, origin.z);
btVector3 toP(origin.x + ray.x, origin.y + ray.y, origin.z + ray.z);
btCollisionWorld::ClosestRayResultCallback rayCallback(fromP, toP);
m_dynamicsWorld->rayTest(fromP , toP, rayCallback);
if(rayCallback.hasHit()) {
btRigidBody * body = (btRigidBody *)btRigidBody::upcast(rayCallback.m_collisionObject);
if(body) {
body->setActivationState(DISABLE_DEACTIVATION);
btVector3 pickPos = rayCallback.m_hitPointWorld;
hitP.x = pickPos.getX();
hitP.y = pickPos.getY();
hitP.z = pickPos.getZ();
m_activeBody = body;
return 1;
}
}
return 0;
}
void CHolonomicPathNode::interpolate(CHolonomicPathNode* from, float t, float angularCoeff) {
float vect[7];
if (_nodeType == sim_holonomicpathplanning_xy) {
vect[0] = values[0] - from->values[0];
vect[1] = values[1] - from->values[1];
float l = sqrtf(vect[0] * vect[0] + vect[1] * vect[1]);
float bound = t / l;
values[0] = vect[0] * bound + from->values[0];
values[1] = vect[1] * bound + from->values[1];
} else if (_nodeType == sim_holonomicpathplanning_xyz) {
vect[0] = values[0] - from->values[0];
vect[1] = values[1] - from->values[1];
vect[2] = values[2] - from->values[2];
float l = sqrtf(vect[0] * vect[0] + vect[1] * vect[1] + vect[2] * vect[2]);
float bound = t / l;
/*
values[0] = vect[0] * bound + from->values[0];
values[1] = vect[1] * bound + from->values[1];
values[2] = vect[2] * bound + from->values[2];
*/
for (int i = 0; i < 3; i++) {
values[i] = from->values[i] * (1.0 - bound) + values[i] * bound;
}
} else if (_nodeType == sim_holonomicpathplanning_xyg) {
vect[0] = values[0] - from->values[0];
vect[1] = values[1] - from->values[1];
vect[2] = CPathPlanningInterface::getNormalizedAngle(values[2] - from->values[2]);
float l = vect[0] * vect[0] + vect[1] * vect[1];
l = sqrtf(l + vect[2] * angularCoeff * vect[2] * angularCoeff);
float bound = t / l;
values[0] = vect[0] * bound + from->values[0];
values[1] = vect[1] * bound + from->values[1];
values[2] = CPathPlanningInterface::getNormalizedAngle(
vect[2] * bound + from->values[2]);
} else if (_nodeType == sim_holonomicpathplanning_xyzabg) {
vect[0] = values[0] - from->values[0];
vect[1] = values[1] - from->values[1];
vect[2] = values[2] - from->values[2];
C4Vector toP, fromP;
C3Vector dum;
getAllValues(dum, toP);
from->getAllValues(dum, fromP);
C4Vector diff(fromP.getInverse()*toP);
vect[3] = diff(0);
vect[4] = diff(1);
vect[5] = diff(2);
vect[6] = diff(3);
float ap = angularCoeff * fromP.getAngleBetweenQuaternions(toP);
float l = vect[0] * vect[0] + vect[1] * vect[1] + vect[2] * vect[2];
l = sqrtf(l + ap * ap);
float bound = t / l;
values[0] = vect[0] * bound + from->values[0];
values[1] = vect[1] * bound + from->values[1];
values[2] = vect[2] * bound + from->values[2];
C4Vector q;
q.setIdentity();
fromP.buildInterpolation(q, diff, bound);
values[3] = fromP(0);
values[4] = fromP(1);
values[5] = fromP(2);
values[6] = fromP(3);
}
}
