void RigidBody::sim_step(double dt)
{
odeint::integrate(boost::ref(*this), internalState, 0.0, dt, dt);
position(0) = internalState[0];
position(1) = internalState[1];
position(2) = internalState[2];
velocity(0) = internalState[3];
velocity(1) = internalState[4];
velocity(2) = internalState[5];
attitude.x() = internalState[6];
attitude.y() = internalState[7];
attitude.z() = internalState[8];
attitude.w() = internalState[9];
attitude.normalize();
angularVelocity(0) = internalState[10];
angularVelocity(1) = internalState[11];
angularVelocity(2) = internalState[12];
updateInternalState();
ticks += 1;
}
/**
* aligns the body x axis with the velocity vector
*/
void alignVelocityXAxis(double vel_tol = .01)
{
Eigen::Quaterniond wind_axes_to_body_axes = Eigen::Quaterniond::Identity(); //transforms vectors in wind frame to vectors in body frame
if (velocity().norm() > vel_tol) {
wind_axes_to_body_axes.setFromTwoVectors(Eigen::Vector3d::UnitX(), velocity());
}
Eigen::Matrix3d body_axes_to_wind_axes = wind_axes_to_body_axes.toRotationMatrix().transpose();
angularVelocity() = body_axes_to_wind_axes * angularVelocity();
velocity() = body_axes_to_wind_axes * velocity();
acceleration() = body_axes_to_wind_axes * acceleration();
orientation() = orientation() * wind_axes_to_body_axes;
}
TYPED_TEST(EulerAnglesXyzDiffTest, testFiniteDifference)
{
typedef typename TestFixture::Scalar Scalar;
typedef typename TestFixture::Rotation Rotation;
typedef typename TestFixture::RotationDiff RotationDiff;
typedef typename TestFixture::Vector3 Vector3;
const double dt = 1.0e-3;
for (auto rotation : this->rotations) {
for (auto angularVelocity2 : this->angularVelocities) {
RotationDiff rotationDiff(angularVelocity2.toImplementation());
rot::LocalAngularVelocity<Scalar> angularVelocity(rotation, rotationDiff);
// Finite difference method for checking derivatives
Rotation rotationNext = rotation.boxPlus(dt*rotation.rotate(angularVelocity.vector()));
rotationNext.setUnique();
rotation.setUnique();
Vector3 dn = (rotationNext.toImplementation()-rotation.toImplementation())/dt;
ASSERT_NEAR(rotationDiff.x(),dn(0),1e-3) << " angular velocity: " << angularVelocity << " rotation: " << rotation << " rotationNext: " << rotationNext << " diff: " << rotationDiff << " approxdiff: " << dn.transpose();
ASSERT_NEAR(rotationDiff.y(),dn(1),1e-3) << " angular velocity: " << angularVelocity << "rotation: " << rotation << " rotationNext: " << rotationNext << " diff: " << rotationDiff << " approxdiff: " << dn.transpose();
ASSERT_NEAR(rotationDiff.z(),dn(2),1e-3) << " angular velocity: " << angularVelocity << " rotation: " << rotation << " rotationNext: " << rotationNext << " diff: " << rotationDiff << " approxdiff: " << dn.transpose();
}
}
}
bool MissionControlSpeedFilter::advance(double dt) {
const double maxHeadingVel = maximumBaseTwistInHeadingFrame_.getTranslationalVelocity().x();
filteredVelocities_[0].update(unfilteredBaseTwistInHeadingFrame_.getTranslationalVelocity().x());
double headingVel = filteredVelocities_[0].val();
// boundToRange(&headingVel, -maxHeadingVel, maxHeadingVel);
headingVel = mapInRange(headingVel, -1.0, 1.0, -maxHeadingVel, maxHeadingVel);
const double maxLateralVel = maximumBaseTwistInHeadingFrame_.getTranslationalVelocity().y();
filteredVelocities_[1].update(unfilteredBaseTwistInHeadingFrame_.getTranslationalVelocity().y());
double lateralVel = filteredVelocities_[1].val();
// boundToRange(&lateralVel, -maxLateralVel, maxLateralVel);
lateralVel = mapInRange(lateralVel, -1.0, 1.0, -maxLateralVel, maxLateralVel);
LinearVelocity linearVelocity(headingVel, lateralVel, 0.0);
const double maxTurningVel = maximumBaseTwistInHeadingFrame_.getRotationalVelocity().z();
filteredVelocities_[2].update(unfilteredBaseTwistInHeadingFrame_.getRotationalVelocity().z());
double turningVel = filteredVelocities_[2].val();
//boundToRange(&turningVel, -maxTurningVel, maxTurningVel);
turningVel = mapInRange(turningVel, -1.0, 1.0, -maxTurningVel, maxTurningVel);
LocalAngularVelocity angularVelocity(0.0, 0.0, turningVel);
baseTwistInHeadingFrame_ = Twist(linearVelocity, angularVelocity);
/* -------- Position -------- */
// desiredPositionOffsetInWorldFrame_.z() = 0.5*(maxHeight-minHeight)*(joyStick->getVertical()-minHeight) + maxHeight;
boundToRange(&desiredPositionOffsetInWorldFrame_.x(), minimalDesiredPositionOffsetInWorldFrame_.x(), maximalDesiredPositionOffsetInWorldFrame_.x());
boundToRange(&desiredPositionOffsetInWorldFrame_.y(), minimalDesiredPositionOffsetInWorldFrame_.y(), maximalDesiredPositionOffsetInWorldFrame_.y());
boundToRange(&desiredPositionOffsetInWorldFrame_.z(), minimalDesiredPositionOffsetInWorldFrame_.z(), maximalDesiredPositionOffsetInWorldFrame_.z());
/* -------- Orientation -------- */
EulerAnglesZyx desEulerAnglesZyx(desiredOrientationHeadingToBase_);
desEulerAnglesZyx.setUnique();
EulerAnglesZyx minEulerAnglesZyx(minimalOrientationHeadingToBase_);
minEulerAnglesZyx.setUnique();
EulerAnglesZyx maxEulerAnglesZyx(maximalOrientationHeadingToBase_);
maxEulerAnglesZyx.setUnique();
double value = desEulerAnglesZyx.roll();
boundToRange(&value, minEulerAnglesZyx.roll(), maxEulerAnglesZyx.roll());
desEulerAnglesZyx.setRoll(value);
value = desEulerAnglesZyx.pitch();
boundToRange(&value, minEulerAnglesZyx.pitch(), maxEulerAnglesZyx.pitch());
desEulerAnglesZyx.setPitch(value);
value = desEulerAnglesZyx.yaw();
boundToRange(&value, minEulerAnglesZyx.yaw(), maxEulerAnglesZyx.yaw());
desEulerAnglesZyx.setYaw(value);
desiredOrientationHeadingToBase_(desEulerAnglesZyx.getUnique());
return true;
}
//http://klas-physics.googlecode.com/svn/trunk/src/general/Integrator.cpp (reference)
void CVX_Voxel::timeStep(float dt)
{
previousDt = dt;
if (dt == 0.0f) return;
if (dofIsAllSet(dofFixed)){ //if this voxel is fixed, no need to change its location
haltMotion();
return;
}
//Translation
Vec3D<> curForce = force();
linMom += curForce*dt;
if (dofIsSet(dofFixed, X_TRANSLATE)){linMom.x=0;}
if (dofIsSet(dofFixed, Y_TRANSLATE)){linMom.y=0;}
if (dofIsSet(dofFixed, Z_TRANSLATE)){linMom.z=0;}
Vec3D<double> translate(linMom*(dt*mat->_massInverse)); //movement of the voxel this timestep
if (isFloorEnabled() && floorPenetration() > 0){ //we must catch a slowing voxel here since it all boils down to needing access to the dt of this timestep.
vfloat work = curForce.x*translate.x + curForce.y*translate.y; //F dot disp
if(kineticEnergy() + work < 0) setFloorStaticFriction(true); //this checks for a change of direction according to the work-energy principle
if (isFloorStaticFriction()){ //if we're in a state of static friction, zero out all horizontal motion
linMom.x = linMom.y = 0;
translate.x = translate.y = 0;
}
}
pos += translate;
//Rotation
Vec3D<> curMoment = moment();
angMom += curMoment*dt;
if (dofIsSet(dofFixed, X_ROTATE)){angMom.x=0;}
if (dofIsSet(dofFixed, Y_ROTATE)){angMom.y=0;}
if (dofIsSet(dofFixed, Z_ROTATE)){angMom.z=0;}
orient = CQuat<>(angularVelocity()*dt)*orient; //update the orientation
// if(pSim->StatToCalc & CALCSTAT_KINE) KineticEnergy = 0.5*Mass*Vel.Length2() + 0.5*Inertia*AngVel.Length2(); //1/2 m v^2
// if(pSim->StatToCalc & CALCSTAT_PRESSURE) {
//// vfloat VolumetricStrain = GetVoxelStrain(AXIS_X) + GetVoxelStrain(AXIS_Y) + GetVoxelStrain(AXIS_Z);
// vfloat VolumetricStrain = strain(false).x+strain(false).y+strain(false).z;
// Pressure = - _pMat->GetElasticMod()*VolumetricStrain/(3*(1-2*_pMat->GetPoissonsRatio())); //http://www.colorado.edu/engineering/CAS/courses.d/Structures.d/IAST.Lect05.d/IAST.Lect05.pdf
// }
//
// updatePoissonsstrain();
//
//
}
void PhysicsSystem3D::OnUpdate(float elapsedTime)
{
if (!m_world)
return;
m_world->Step(elapsedTime);
for (const Ndk::EntityHandle& entity : m_dynamicObjects)
{
NodeComponent& node = entity->GetComponent<NodeComponent>();
PhysicsComponent3D& phys = entity->GetComponent<PhysicsComponent3D>();
Nz::RigidBody3D* physObj = phys.GetRigidBody();
node.SetRotation(physObj->GetRotation(), Nz::CoordSys_Global);
node.SetPosition(physObj->GetPosition(), Nz::CoordSys_Global);
}
float invElapsedTime = 1.f / elapsedTime;
for (const Ndk::EntityHandle& entity : m_staticObjects)
{
CollisionComponent3D& collision = entity->GetComponent<CollisionComponent3D>();
NodeComponent& node = entity->GetComponent<NodeComponent>();
Nz::RigidBody3D* physObj = collision.GetStaticBody();
Nz::Quaternionf oldRotation = physObj->GetRotation();
Nz::Vector3f oldPosition = physObj->GetPosition();
Nz::Quaternionf newRotation = node.GetRotation(Nz::CoordSys_Global);
Nz::Vector3f newPosition = node.GetPosition(Nz::CoordSys_Global);
// To move static objects and ensure their collisions, we have to specify them a velocity
// (/!\: the physical motor does not apply the speed on static objects)
if (newPosition != oldPosition)
{
physObj->SetPosition(newPosition);
physObj->SetLinearVelocity((newPosition - oldPosition) * invElapsedTime);
}
else
physObj->SetLinearVelocity(Nz::Vector3f::Zero());
if (newRotation != oldRotation)
{
Nz::Quaternionf transition = newRotation * oldRotation.GetConjugate();
Nz::EulerAnglesf angles = transition.ToEulerAngles();
Nz::Vector3f angularVelocity(Nz::ToRadians(angles.pitch * invElapsedTime),
Nz::ToRadians(angles.yaw * invElapsedTime),
Nz::ToRadians(angles.roll * invElapsedTime));
physObj->SetRotation(oldRotation);
physObj->SetAngularVelocity(angularVelocity);
}
else
physObj->SetAngularVelocity(Nz::Vector3f::Zero());
}
}
void Box2DBody::setAngularVelocity(float velocity)
{
if (angularVelocity() == velocity)
return;
mBodyDef.angularVelocity = toRadians(velocity);
if (mBody)
mBody->SetAngularVelocity(mBodyDef.angularVelocity);
emit angularVelocityChanged();
}
void RigidBody::updateInternalState(void)
{
internalState[0] = position(0);
internalState[1] = position(1);
internalState[2] = position(2);
internalState[3] = velocity(0);
internalState[4] = velocity(1);
internalState[5] = velocity(2);
//internalState[6] = acceleration(0);
//internalState[7] = acceleration(1);
//internalState[8] = acceleration(2);
internalState[6] = attitude.x();
internalState[7] = attitude.y();
internalState[8] = attitude.z();
internalState[9] = attitude.w();
internalState[10] = angularVelocity(0);
internalState[11] = angularVelocity(1);
internalState[12] = angularVelocity(2);
}
Vec3D<> CVX_Voxel::moment()
{
//moments from internal bonds
Vec3D<> totalMoment(0,0,0);
for (int i=0; i<6; i++){
if (links[i]) totalMoment += links[i]->moment(isNegative((linkDirection)i)); //total force in LCS
}
totalMoment = orient.RotateVec3D(totalMoment);
//other moments
totalMoment += extMoment; //external moments
totalMoment -= angularVelocity()*mat->globalDampingRotateC(); //global damping
return totalMoment;
}
void Physics::update(int delta){
float dt=delta/1000.0f;
// float mindt = 0.005f;
// float leftover=(dt>mindt)?dt-mindt:0;
//calculate the amount of force being applied to object for a split second
Vector appliedForce(impulseForce_+bodyForce_);
acceleration_=appliedForce/mass_;
//reduce the impulse force to 0
// impulse moments
Matrix R = rotation();
Matrix RT = R.transpose();
Vector ml = moment_ * RT;
Vector wl = angularVelocity() * RT;
setAngularAcceleration((ml - cross(wl*momentInertia_ ,wl)) * invMomentInertia_ * R);
updateKinematics(dt);
moment_ = Vector();
impulseForce_=Vector();
//setAngularAcceleration(Vector());
//calculate a new acceleration for the rest of the frame based soley on the body force
//acceleration_=bodyForce_/mass_;
}
void SixenseManager::update(float deltaTime) {
#ifdef HAVE_SIXENSE
Hand* hand = DependencyManager::get<AvatarManager>()->getMyAvatar()->getHand();
if (_isInitialized && _isEnabled) {
#ifdef __APPLE__
SixenseBaseFunction sixenseGetNumActiveControllers =
(SixenseBaseFunction) _sixenseLibrary->resolve("sixenseGetNumActiveControllers");
#endif
if (sixenseGetNumActiveControllers() == 0) {
_hydrasConnected = false;
return;
}
PerformanceTimer perfTimer("sixense");
if (!_hydrasConnected) {
_hydrasConnected = true;
UserActivityLogger::getInstance().connectedDevice("spatial_controller", "hydra");
}
#ifdef __APPLE__
SixenseBaseFunction sixenseGetMaxControllers =
(SixenseBaseFunction) _sixenseLibrary->resolve("sixenseGetMaxControllers");
#endif
int maxControllers = sixenseGetMaxControllers();
// we only support two controllers
sixenseControllerData controllers[2];
#ifdef __APPLE__
SixenseTakeIntFunction sixenseIsControllerEnabled =
(SixenseTakeIntFunction) _sixenseLibrary->resolve("sixenseIsControllerEnabled");
SixenseTakeIntAndSixenseControllerData sixenseGetNewestData =
(SixenseTakeIntAndSixenseControllerData) _sixenseLibrary->resolve("sixenseGetNewestData");
#endif
int numControllersAtBase = 0;
int numActiveControllers = 0;
for (int i = 0; i < maxControllers && numActiveControllers < 2; i++) {
if (!sixenseIsControllerEnabled(i)) {
continue;
}
sixenseControllerData* data = controllers + numActiveControllers;
++numActiveControllers;
sixenseGetNewestData(i, data);
// Set palm position and normal based on Hydra position/orientation
// Either find a palm matching the sixense controller, or make a new one
PalmData* palm;
bool foundHand = false;
for (size_t j = 0; j < hand->getNumPalms(); j++) {
if (hand->getPalms()[j].getSixenseID() == data->controller_index) {
palm = &(hand->getPalms()[j]);
foundHand = true;
}
}
if (!foundHand) {
PalmData newPalm(hand);
hand->getPalms().push_back(newPalm);
palm = &(hand->getPalms()[hand->getNumPalms() - 1]);
palm->setSixenseID(data->controller_index);
qCDebug(interfaceapp, "Found new Sixense controller, ID %i", data->controller_index);
}
// Disable the hands (and return to default pose) if both controllers are at base station
if (foundHand) {
palm->setActive(!_controllersAtBase);
} else {
palm->setActive(false); // if this isn't a Sixsense ID palm, always make it inactive
}
// Read controller buttons and joystick into the hand
palm->setControllerButtons(data->buttons);
palm->setTrigger(data->trigger);
palm->setJoystick(data->joystick_x, data->joystick_y);
// Emulate the mouse so we can use scripts
if (Menu::getInstance()->isOptionChecked(MenuOption::SixenseMouseInput) && !_controllersAtBase) {
emulateMouse(palm, numActiveControllers - 1);
}
// NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters.
glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
position *= METERS_PER_MILLIMETER;
// Check to see if this hand/controller is on the base
const float CONTROLLER_AT_BASE_DISTANCE = 0.075f;
if (glm::length(position) < CONTROLLER_AT_BASE_DISTANCE) {
numControllersAtBase++;
}
// Transform the measured position into body frame.
glm::vec3 neck = _neckBase;
// Zeroing y component of the "neck" effectively raises the measured position a little bit.
neck.y = 0.0f;
position = _orbRotation * (position - neck);
// Rotation of Palm
glm::quat rotation(data->rot_quat[3], -data->rot_quat[0], data->rot_quat[1], -data->rot_quat[2]);
rotation = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f)) * _orbRotation * rotation;
// Compute current velocity from position change
glm::vec3 rawVelocity;
if (deltaTime > 0.0f) {
rawVelocity = (position - palm->getRawPosition()) / deltaTime;
} else {
rawVelocity = glm::vec3(0.0f);
}
palm->setRawVelocity(rawVelocity); // meters/sec
// adjustment for hydra controllers fit into hands
float sign = (i == 0) ? -1.0f : 1.0f;
rotation *= glm::angleAxis(sign * PI/4.0f, glm::vec3(0.0f, 0.0f, 1.0f));
// Angular Velocity of Palm
glm::quat deltaRotation = rotation * glm::inverse(palm->getRawRotation());
glm::vec3 angularVelocity(0.0f);
float rotationAngle = glm::angle(deltaRotation);
if ((rotationAngle > EPSILON) && (deltaTime > 0.0f)) {
angularVelocity = glm::normalize(glm::axis(deltaRotation));
angularVelocity *= (rotationAngle / deltaTime);
palm->setRawAngularVelocity(angularVelocity);
} else {
palm->setRawAngularVelocity(glm::vec3(0.0f));
}
if (_lowVelocityFilter) {
// Use a velocity sensitive filter to damp small motions and preserve large ones with
// no latency.
float velocityFilter = glm::clamp(1.0f - glm::length(rawVelocity), 0.0f, 1.0f);
position = palm->getRawPosition() * velocityFilter + position * (1.0f - velocityFilter);
rotation = safeMix(palm->getRawRotation(), rotation, 1.0f - velocityFilter);
palm->setRawPosition(position);
palm->setRawRotation(rotation);
} else {
palm->setRawPosition(position);
palm->setRawRotation(rotation);
}
// Store the one fingertip in the palm structure so we can track velocity
const float FINGER_LENGTH = 0.3f; // meters
const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
glm::vec3 oldTipPosition = palm->getTipRawPosition();
if (deltaTime > 0.0f) {
palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime);
} else {
palm->setTipVelocity(glm::vec3(0.0f));
}
palm->setTipPosition(newTipPosition);
}
if (numActiveControllers == 2) {
updateCalibration(controllers);
}
_controllersAtBase = (numControllersAtBase == 2);
}
#endif // HAVE_SIXENSE
}
void integrateForwardConstantVelocity(double time)
{
Eigen::Affine3d trans = getTransTwist(angularVelocity(), velocity(), time);
this->orientation() = this->orientation() * trans.rotation();
this->position() = this->position() + this->orientation() * trans.translation();
}
Vector Physics::velocityAtPoint(const Vector& p)const{
return velocity() + cross(angularVelocity(), p-worldCentreOfMass());
}
// tanVelocity returns the tangential velocity of the frame in world space.
Vector Physics::tanVelocity() const{
return cross(angularVelocity(),position())-tanVelocityReference();
}
