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(); }