/** * 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); }