void KinematicCharacterController::playerStep(btCollisionWorld* collisionWorld, btScalar dt)
{
// printf("playerStep(): ");
// printf(" dt = %f", dt);
// quick check...
if (!m_useWalkDirection && (m_velocityTimeInterval <= 0.0 || m_walkDirection.fuzzyZero())) {
// printf("\n");
return; // no motion
}
m_wasOnGround = onGround();
// Update fall velocity.
m_verticalVelocity -= m_gravity * dt;
if (m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed)
{
m_verticalVelocity = m_jumpSpeed;
}
if (m_verticalVelocity < 0.0 && btFabs(m_verticalVelocity) > btFabs(m_fallSpeed))
{
m_verticalVelocity = -btFabs(m_fallSpeed);
}
m_verticalOffset = m_verticalVelocity * dt;
btTransform xform;
xform = m_ghostObject->getWorldTransform();
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
stepUp(collisionWorld);
if (m_useWalkDirection) {
stepForwardAndStrafe(collisionWorld, m_walkDirection);
}
else {
//printf(" time: %f", m_velocityTimeInterval);
// still have some time left for moving!
btScalar dtMoving =
(dt < m_velocityTimeInterval) ? dt : m_velocityTimeInterval;
m_velocityTimeInterval -= dt;
// how far will we move while we are moving?
btVector3 move = m_walkDirection * dtMoving;
//printf(" dtMoving: %f", dtMoving);
// okay, step
stepForwardAndStrafe(collisionWorld, move);
}
stepDown(collisionWorld, dt);
// printf("\n");
xform.setOrigin(m_currentPosition);
m_ghostObject->setWorldTransform(xform);
}
// --------------------------------------------------------------------------
// ARDrone::setFlatTrim()
// Description : Set a reference of the horizontal plane.
// Return value : NONE
// --------------------------------------------------------------------------
void ARDrone::setFlatTrim(void)
{
if (onGround()) {
// Send flat trim command
if (mutexCommand) pthread_mutex_lock(mutexCommand);
sockCommand.sendf("AT*FTRIM=%d\r", ++seq);
if (mutexCommand) pthread_mutex_unlock(mutexCommand);
}
}
/////////////////////////////////////////////////////////////////////////////
// Calibrate magnetos (0=magneto, 1=device)
/////////////////////////////////////////////////////////////////////////////
void CCustomDrone::Calibrate(int iDevice)
{
if (!onGround())
{
if (mutexCommand) pthread_mutex_lock(mutexCommand);;
sockCommand.sendf("AT*CALIB=%d,%d\r", seq++, iDevice);
if (mutexCommand) pthread_mutex_unlock(mutexCommand);;
}
}
// --------------------------------------------------------------------------
// ARDrone::setCalibration(Device ID)
// Description : Calibrate AR.Drone's magnetometer.
// Return value : NONE
// --------------------------------------------------------------------------
void ARDrone::setCalibration(int device)
{
if (!onGround()) {
// Send calibration command
if (mutexCommand) pthread_mutex_lock(mutexCommand);
sockCommand.sendf("AT*CALIB=%d,%d\r", ++seq, device);
if (mutexCommand) pthread_mutex_unlock(mutexCommand);
}
}
/////////////////////////////////////////////////////////////////////////////
// Send trim command to the drone
/////////////////////////////////////////////////////////////////////////////
void CCustomDrone::Trim(void)
{
if (onGround())
{
if (mutexCommand) pthread_mutex_lock(mutexCommand);;
sockCommand.sendf("AT*FTRIM=%d,\r", seq++);
if (mutexCommand) pthread_mutex_unlock(mutexCommand);;
msleep(100);
}
}
void DynamicCharacterController::playerStep (btCollisionWorld* dynaWorld,btScalar dt/*,
int forward,
int backward,
int left,
int right,
int jump*/)
{
btTransform xform;
m_rigidBody->getMotionState()->getWorldTransform (xform);
#if 0
/* Handle turning */
if (left)
m_turnAngle -= dt * m_turnVelocity;
if (right)
m_turnAngle += dt * m_turnVelocity;
#endif
xform.setRotation (btQuaternion (btVector3(0.0, 1.0, 0.0), m_turnAngle));
btVector3 linearVelocity = m_rigidBody->getLinearVelocity();
btScalar speed = m_rigidBody->getLinearVelocity().length();
btVector3 forwardDir = xform.getBasis()[2];
forwardDir.normalize ();
btVector3 walkDirection = btVector3(0.0, 0.0, 0.0);
btScalar walkSpeed = m_walkVelocity * dt;
#if 0
if (forward)
walkDirection += forwardDir;
if (backward)
walkDirection -= forwardDir;
#endif
#if 0
if (!forward && !backward && onGround())
{
/* Dampen when on the ground and not being moved by the player */
linearVelocity *= btScalar(0.2);
m_rigidBody->setLinearVelocity (linearVelocity);
} else {
if (speed < m_maxLinearVelocity)
{
btVector3 velocity = linearVelocity + walkDirection * walkSpeed;
m_rigidBody->setLinearVelocity (velocity);
}
}
#endif
m_rigidBody->getMotionState()->setWorldTransform (xform);
m_rigidBody->setCenterOfMassTransform (xform);
}
void DynamicCharacterController::preSimulation(btScalar timeStep) {
if (_enabled && _dynamicsWorld) {
glm::quat rotation = _avatarData->getOrientation();
// TODO: update gravity if up has changed
updateUpAxis(rotation);
glm::vec3 position = _avatarData->getPosition() + rotation * _shapeLocalOffset;
_rigidBody->setWorldTransform(btTransform(glmToBullet(rotation), glmToBullet(position)));
// the rotation is dictated by AvatarData
btTransform xform = _rigidBody->getWorldTransform();
xform.setRotation(glmToBullet(rotation));
_rigidBody->setWorldTransform(xform);
// scan for distant floor
// rayStart is at center of bottom sphere
btVector3 rayStart = xform.getOrigin() - _halfHeight * _currentUp;
// rayEnd is straight down MAX_FALL_HEIGHT
btScalar rayLength = _radius + MAX_FALL_HEIGHT;
btVector3 rayEnd = rayStart - rayLength * _currentUp;
ClosestNotMe rayCallback(_rigidBody);
rayCallback.m_closestHitFraction = 1.0f;
_dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback);
if (rayCallback.hasHit()) {
_floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius;
const btScalar MIN_HOVER_HEIGHT = 3.0f;
if (_isHovering && _floorDistance < MIN_HOVER_HEIGHT && !_isPushingUp) {
setHovering(false);
}
// TODO: use collision events rather than ray-trace test to disable jumping
const btScalar JUMP_PROXIMITY_THRESHOLD = 0.1f * _radius;
if (_floorDistance < JUMP_PROXIMITY_THRESHOLD) {
_isJumping = false;
}
} else {
_floorDistance = FLT_MAX;
setHovering(true);
}
_walkVelocity = glmToBullet(_avatarData->getTargetVelocity());
if (_pendingFlags & PENDING_FLAG_JUMP) {
_pendingFlags &= ~ PENDING_FLAG_JUMP;
if (onGround()) {
_isJumping = true;
btVector3 velocity = _rigidBody->getLinearVelocity();
velocity += _jumpSpeed * _currentUp;
_rigidBody->setLinearVelocity(velocity);
}
}
}
}
// --------------------------------------------------------------------------
// ARDrone::close()
// Description : Finalize
// Return value : NONE
// --------------------------------------------------------------------------
void ARDrone::close(void)
{
// Stop AR.Drone
if (!onGround()) landing();
// Finalize video
finalizeVideo();
// Finalize Navdata
finalizeNavdata();
// Finalize AT command
finalizeCommand();
}
// --------------------------------------------------------------------------
// ARDrone::move3D(X velocity[m/s], Y velocity[m/s], Z velocity[m/s], Rotational speed[rad/s])
// Description : Move the AR.Drone in 3D space.
// Return value : NONE
// --------------------------------------------------------------------------
void ARDrone::move3D(double vx, double vy, double vz, double vr)
{
// AR.Drone is flying
if (!onGround()) {
const float gain = 0.4f;
float v[4] = {-vy*gain, -vx*gain, vz*gain, -vr*gain};
int mode = (fabs(vx) > 0.0 || fabs(vy) > 0.0);
sockCommand.sendf("AT*PCMD=%d,%d,%d,%d,%d,%d\r", seq++, mode, *(int*)(&v[0]), *(int*)(&v[1]), *(int*)(&v[2]), *(int*)(&v[3]));
}
// Reset Watch-Dog every 100ms
if ((cvGetTickCount() - timer) / cvGetTickFrequency() > 100000) {
sockCommand.sendf("AT*COMWDG=%d\r", seq++);
timer = cvGetTickCount();
}
}
void MyCharacterController::preSimulation() {
if (_enabled && _dynamicsWorld) {
// slam body to where it is supposed to be
_rigidBody->setWorldTransform(_avatarBodyTransform);
// scan for distant floor
// rayStart is at center of bottom sphere
btVector3 rayStart = _avatarBodyTransform.getOrigin() - _halfHeight * _currentUp;
// rayEnd is straight down MAX_FALL_HEIGHT
btScalar rayLength = _radius + MAX_FALL_HEIGHT;
btVector3 rayEnd = rayStart - rayLength * _currentUp;
ClosestNotMe rayCallback(_rigidBody);
rayCallback.m_closestHitFraction = 1.0f;
_dynamicsWorld->rayTest(rayStart, rayEnd, rayCallback);
if (rayCallback.hasHit()) {
_floorDistance = rayLength * rayCallback.m_closestHitFraction - _radius;
const btScalar MIN_HOVER_HEIGHT = 3.0f;
if (_isHovering && _floorDistance < MIN_HOVER_HEIGHT && !_isPushingUp) {
setHovering(false);
}
// TODO: use collision events rather than ray-trace test to disable jumping
const btScalar JUMP_PROXIMITY_THRESHOLD = 0.1f * _radius;
if (_floorDistance < JUMP_PROXIMITY_THRESHOLD) {
_isJumping = false;
}
} else {
_floorDistance = FLT_MAX;
setHovering(true);
}
if (_pendingFlags & PENDING_FLAG_JUMP) {
_pendingFlags &= ~ PENDING_FLAG_JUMP;
if (onGround()) {
_isJumping = true;
btVector3 velocity = _rigidBody->getLinearVelocity();
velocity += _jumpSpeed * _currentUp;
_rigidBody->setLinearVelocity(velocity);
}
}
}
_lastStepDuration = 0.0f;
}
// --------------------------------------------------------------------------
// ARDrone::close()
// Description : Finalize
// Return value : NONE
// --------------------------------------------------------------------------
void ARDrone::close(void)
{
// Stop AR.Drone
if (!onGround()) landing();
// Finalize video
finalizeVideo();
// Finalize Navdata
finalizeNavdata();
// Finalize AT command
finalizeCommand();
#if _WIN32
// Finalize WSA
WSACleanup();
#endif
}
// --------------------------------------------------------------------------
// ARDrone::move3D(X velocity[m/s], Y velocity[m/s], Z velocity[m/s], Rotational speed[rad/s])
// Description : Move the AR.Drone in 3D space.
// Return value : NONE
// --------------------------------------------------------------------------
void ARDrone::move3D(double vx, double vy, double vz, double vr)
{
// AR.Drone is flying
if (!onGround()) {
// Command velocities
float v[4] = {-vy*0.2, -vx*0.2, vz*1.0, -vr*0.5};
int mode = (fabs(v[0]) > 0.0 || fabs(v[1]) > 0.0 || fabs(v[2]) > 0.0 || fabs(v[3]) > 0.0);
// Nomarization (-1.0 to +1.0)
for (int i = 0; i < 4; i++) {
if (fabs(v[i]) > 1.0) v[i] /= fabs(v[i]);
}
// Send a command
if (mutexCommand) pthread_mutex_lock(mutexCommand);
sockCommand.sendf("AT*PCMD=%d,%d,%d,%d,%d,%d\r", ++seq, mode, *(int*)(&v[0]), *(int*)(&v[1]), *(int*)(&v[2]), *(int*)(&v[3]));
if (mutexCommand) pthread_mutex_unlock(mutexCommand);
}
}
// --------------------------------------------------------------------------
// ARDrone::close()
// Description : Finalize
// Return value : NONE
// --------------------------------------------------------------------------
void ARDrone::close(void)
{
// Stop AR.Drone
if (!onGround()) landing();
// Finalize Navdata
finalizeNavdata();
// Finalize configuration
finalizeConfig();
// Finalize AT command
finalizeCommand();
// Finalize video
finalizeVideo();
// Finalize WSA
WSACleanup();
}
bool bulletCharacterController::canJump () const
{
return onGround();
}
void MyCharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
btVector3 actualVelocity = _rigidBody->getLinearVelocity();
btScalar actualSpeed = actualVelocity.length();
btVector3 desiredVelocity = _walkVelocity;
btScalar desiredSpeed = desiredVelocity.length();
const btScalar MIN_UP_PUSH = 0.1f;
if (desiredVelocity.dot(_currentUp) < MIN_UP_PUSH) {
_isPushingUp = false;
}
const btScalar MIN_SPEED = 0.001f;
if (_isHovering) {
if (desiredSpeed < MIN_SPEED) {
if (actualSpeed < MIN_SPEED) {
_rigidBody->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f));
} else {
const btScalar HOVER_BRAKING_TIMESCALE = 0.1f;
btScalar tau = glm::max(dt / HOVER_BRAKING_TIMESCALE, 1.0f);
_rigidBody->setLinearVelocity((1.0f - tau) * actualVelocity);
}
} else {
const btScalar HOVER_ACCELERATION_TIMESCALE = 0.1f;
btScalar tau = dt / HOVER_ACCELERATION_TIMESCALE;
_rigidBody->setLinearVelocity(actualVelocity - tau * (actualVelocity - desiredVelocity));
}
} else {
if (onGround()) {
// walking on ground
if (desiredSpeed < MIN_SPEED) {
if (actualSpeed < MIN_SPEED) {
_rigidBody->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f));
} else {
const btScalar HOVER_BRAKING_TIMESCALE = 0.1f;
btScalar tau = dt / HOVER_BRAKING_TIMESCALE;
_rigidBody->setLinearVelocity((1.0f - tau) * actualVelocity);
}
} else {
// TODO: modify desiredVelocity using floor normal
const btScalar WALK_ACCELERATION_TIMESCALE = 0.1f;
btScalar tau = dt / WALK_ACCELERATION_TIMESCALE;
btVector3 velocityCorrection = tau * (desiredVelocity - actualVelocity);
// subtract vertical component
velocityCorrection -= velocityCorrection.dot(_currentUp) * _currentUp;
_rigidBody->setLinearVelocity(actualVelocity + velocityCorrection);
}
} else {
// transitioning to flying
btVector3 velocityCorrection = desiredVelocity - actualVelocity;
const btScalar FLY_ACCELERATION_TIMESCALE = 0.2f;
btScalar tau = dt / FLY_ACCELERATION_TIMESCALE;
if (!_isPushingUp) {
// actually falling --> compute a different velocity attenuation factor
const btScalar FALL_ACCELERATION_TIMESCALE = 2.0f;
tau = dt / FALL_ACCELERATION_TIMESCALE;
// zero vertical component
velocityCorrection -= velocityCorrection.dot(_currentUp) * _currentUp;
}
_rigidBody->setLinearVelocity(actualVelocity + tau * velocityCorrection);
}
}
// Rather than add _hmdVelocity to the velocity of the RigidBody, we explicitly teleport
// the RigidBody forward according to the formula: distance = rate * time
if (_hmdVelocity.length2() > 0.0f) {
btTransform bodyTransform = _rigidBody->getWorldTransform();
bodyTransform.setOrigin(bodyTransform.getOrigin() + dt * _hmdVelocity);
_rigidBody->setWorldTransform(bodyTransform);
}
// MyAvatar will ask us how far we stepped for HMD motion, which will depend on how
// much time has accumulated in _lastStepDuration.
_lastStepDuration += dt;
}
void DynamicCharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt) {
btVector3 actualVelocity = _rigidBody->getLinearVelocity();
btScalar actualSpeed = actualVelocity.length();
btVector3 desiredVelocity = _walkVelocity;
btScalar desiredSpeed = desiredVelocity.length();
const btScalar MIN_UP_PUSH = 0.1f;
if (desiredVelocity.dot(_currentUp) < MIN_UP_PUSH) {
_isPushingUp = false;
}
const btScalar MIN_SPEED = 0.001f;
if (_isHovering) {
if (desiredSpeed < MIN_SPEED) {
if (actualSpeed < MIN_SPEED) {
_rigidBody->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f));
} else {
const btScalar HOVER_BRAKING_TIMESCALE = 0.1f;
btScalar tau = glm::max(dt / HOVER_BRAKING_TIMESCALE, 1.0f);
_rigidBody->setLinearVelocity((1.0f - tau) * actualVelocity);
}
} else {
const btScalar HOVER_ACCELERATION_TIMESCALE = 0.1f;
btScalar tau = dt / HOVER_ACCELERATION_TIMESCALE;
_rigidBody->setLinearVelocity(actualVelocity - tau * (actualVelocity - desiredVelocity));
}
} else {
if (onGround()) {
// walking on ground
if (desiredSpeed < MIN_SPEED) {
if (actualSpeed < MIN_SPEED) {
_rigidBody->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f));
} else {
const btScalar HOVER_BRAKING_TIMESCALE = 0.1f;
btScalar tau = dt / HOVER_BRAKING_TIMESCALE;
_rigidBody->setLinearVelocity((1.0f - tau) * actualVelocity);
}
} else {
// TODO: modify desiredVelocity using floor normal
const btScalar WALK_ACCELERATION_TIMESCALE = 0.1f;
btScalar tau = dt / WALK_ACCELERATION_TIMESCALE;
btVector3 velocityCorrection = tau * (desiredVelocity - actualVelocity);
// subtract vertical component
velocityCorrection -= velocityCorrection.dot(_currentUp) * _currentUp;
_rigidBody->setLinearVelocity(actualVelocity + velocityCorrection);
}
} else {
// transitioning to flying
btVector3 velocityCorrection = desiredVelocity - actualVelocity;
const btScalar FLY_ACCELERATION_TIMESCALE = 0.2f;
btScalar tau = dt / FLY_ACCELERATION_TIMESCALE;
if (!_isPushingUp) {
// actually falling --> compute a different velocity attenuation factor
const btScalar FALL_ACCELERATION_TIMESCALE = 2.0f;
tau = dt / FALL_ACCELERATION_TIMESCALE;
// zero vertical component
velocityCorrection -= velocityCorrection.dot(_currentUp) * _currentUp;
}
_rigidBody->setLinearVelocity(actualVelocity + tau * velocityCorrection);
}
}
}
bool DynamicCharacterController::canJump () const
{
return onGround();
}
bool btKinematicCharacterController::canJump () const
{
return onGround();
}
void btKinematicCharacterController::playerStep( btCollisionWorld* collisionWorld, btScalar dt )
{
BT_PROFILE( "playerStep" );
if( !m_useWalkDirection && m_velocityTimeInterval <= btScalar( 0.0 ) )
return;
bool wasOnGround = onGround();
// Handle the gravity
//
m_verticalVelocity -= m_gravity * dt;
if( m_verticalVelocity > 0.0 && m_verticalVelocity > m_jumpSpeed )
m_verticalVelocity = m_jumpSpeed;
if( m_verticalVelocity < 0.0 && btFabs( m_verticalVelocity ) > btFabs( m_fallSpeed ) )
m_verticalVelocity = -btFabs( m_fallSpeed );
m_verticalOffset = m_verticalVelocity * dt;
// This forced stepping up can cause problems when the character
// walks (jump in fact...) under too low ceilings.
//
btVector3 currentPosition = externalGhostObject->getWorldTransform().getOrigin();
btScalar currentStepOffset;
currentPosition = stepUp( collisionWorld, currentPosition, currentStepOffset );
// Move in the air and slide against the walls ignoring the stair steps.
//
if( m_useWalkDirection )
currentPosition = stepForwardAndStrafe( collisionWorld, currentPosition, m_walkDirection );
else
{
btScalar dtMoving = ( dt < m_velocityTimeInterval ) ? dt : m_velocityTimeInterval;
m_velocityTimeInterval -= dt;
// How far will we move while we are moving ?
//
btVector3 moveDirection = m_walkDirection * dtMoving;
currentPosition = stepForwardAndStrafe( collisionWorld, currentPosition, moveDirection );
}
// Finally find the ground.
//
currentStepOffset = addFallOffset( wasOnGround, currentStepOffset, dt );
currentPosition = stepDown( collisionWorld, currentPosition, currentStepOffset );
// Apply the new position to the collision objects.
//
btTransform tranform;
tranform = externalGhostObject->getWorldTransform();
tranform.setOrigin( currentPosition );
externalGhostObject->setWorldTransform( tranform );
internalGhostObject->setWorldTransform( tranform );
}
