/**
* create and score a trajectory given the current pose of the robot and selected velocities
*/
void CalibrateAction::generateTrajectory(
double x, double y, double theta,
double vx, double vy, double vtheta, double sim_time_, double vx_samp, double vy_samp, double vtheta_samp,
double acc_x, double acc_y, double acc_theta, Trajectory& traj) {
double x_i = x;
double y_i = y;
double theta_i = theta;
double vx_i, vy_i, vtheta_i;
vx_i = vx;
vy_i = vy;
vtheta_i = vtheta;
//compute the number of steps we must take along this trajectory to be "safe"
int num_steps = int(sim_time_ / traj_sim_granularity_ + 0.5);
//we at least want to take one step... even if we won't move, we want to score our current position
if(num_steps == 0) {
num_steps = 1;
}
double dt = sim_time_ / num_steps;
//create a potential trajectory
traj.resetPoints();
traj.xv_ = vx_i;
traj.yv_ = vy_i;
traj.thetav_ = vtheta_i;
for(int i = 0; i < num_steps; ++i){
//the point is legal... add it to the trajectory
traj.addPoint(x_i, y_i, theta_i);
//calculate velocities
vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);
//calculate positions
x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);
} // end for i < numsteps
}
void CharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
btVector3 velocity = _rigidBody->getLinearVelocity() - _parentVelocity;
computeNewVelocity(dt, velocity);
_rigidBody->setLinearVelocity(velocity + _parentVelocity);
// Dynamicaly compute a follow velocity to move this body toward the _followDesiredBodyTransform.
// Rather than add this velocity to velocity the RigidBody, we explicitly teleport the RigidBody towards its goal.
// This mirrors the computation done in MyAvatar::FollowHelper::postPhysicsUpdate().
const float MINIMUM_TIME_REMAINING = 0.005f;
const float MAX_DISPLACEMENT = 0.5f * _radius;
_followTimeRemaining -= dt;
if (_followTimeRemaining >= MINIMUM_TIME_REMAINING) {
btTransform bodyTransform = _rigidBody->getWorldTransform();
btVector3 startPos = bodyTransform.getOrigin();
btVector3 deltaPos = _followDesiredBodyTransform.getOrigin() - startPos;
btVector3 vel = deltaPos / _followTimeRemaining;
btVector3 linearDisplacement = clampLength(vel * dt, MAX_DISPLACEMENT); // clamp displacement to prevent tunneling.
btVector3 endPos = startPos + linearDisplacement;
btQuaternion startRot = bodyTransform.getRotation();
glm::vec2 currentFacing = getFacingDir2D(bulletToGLM(startRot));
glm::vec2 currentRight(currentFacing.y, -currentFacing.x);
glm::vec2 desiredFacing = getFacingDir2D(bulletToGLM(_followDesiredBodyTransform.getRotation()));
float deltaAngle = acosf(glm::clamp(glm::dot(currentFacing, desiredFacing), -1.0f, 1.0f));
float angularSpeed = deltaAngle / _followTimeRemaining;
float sign = copysignf(1.0f, glm::dot(desiredFacing, currentRight));
btQuaternion angularDisplacement = btQuaternion(btVector3(0.0f, 1.0f, 0.0f), sign * angularSpeed * dt);
btQuaternion endRot = angularDisplacement * startRot;
// in order to accumulate displacement of avatar position, we need to take _shapeLocalOffset into account.
btVector3 shapeLocalOffset = glmToBullet(_shapeLocalOffset);
btVector3 swingDisplacement = rotateVector(endRot, -shapeLocalOffset) - rotateVector(startRot, -shapeLocalOffset);
_followLinearDisplacement = linearDisplacement + swingDisplacement + _followLinearDisplacement;
_followAngularDisplacement = angularDisplacement * _followAngularDisplacement;
_rigidBody->setWorldTransform(btTransform(endRot, endPos));
}
_followTime += dt;
}
void CharacterController::computeNewVelocity(btScalar dt, glm::vec3& velocity) {
btVector3 btVelocity = glmToBullet(velocity);
computeNewVelocity(dt, btVelocity);
velocity = bulletToGLM(btVelocity);
}
