btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB) :btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB) { m_pivotInA = frameInA.getOrigin(); m_pivotInB = frameInB.getOrigin(); m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse(); }
void MotionState::setWorldTransform(const btTransform& worldTrans) { btVector3 bPosition = worldTrans.getOrigin(); btVector3 bAxis = worldTrans.getRotation().getAxis(); Rad bRotation(worldTrans.getRotation().getAngle()); /** @todo Verify that all objects have common parent */ transformation.resetTransformation() .rotate(bRotation, Vector3(bAxis).normalized()) .translate(Vector3(bPosition)); }
Transform SWPhysics::toSWTransform(btTransform& o) { Transform ret; Vector3f trans(o.getOrigin().getX(), o.getOrigin().getY(), o.getOrigin().getZ()); Vector3f rot(o.getRotation().getX(), o.getRotation().getY(), o.getOrigin().getZ()); Vector3f scal(1); ret.setTranslate(trans); ret.setRotate(rot); ret.setScale(scal); return ret; }
// This callback is invoked by the physics simulation at the end of each simulation step... // iff the corresponding RigidBody is DYNAMIC and has moved. void EntityMotionState::setWorldTransform(const btTransform& worldTrans) { if (!_entity) { return; } measureBodyAcceleration(); _entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset()); _entity->setRotation(bulletToGLM(worldTrans.getRotation())); _entity->setVelocity(getBodyLinearVelocity()); _entity->setAngularVelocity(getBodyAngularVelocity()); _entity->setLastSimulated(usecTimestampNow()); if (_entity->getSimulatorID().isNull()) { _loopsWithoutOwner++; const uint32_t OWNERSHIP_BID_DELAY = 50; if (_loopsWithoutOwner > OWNERSHIP_BID_DELAY) { //qDebug() << "Warning -- claiming something I saw moving." << getName(); _candidateForOwnership = true; } } else { _loopsWithoutOwner = 0; } #ifdef WANT_DEBUG quint64 now = usecTimestampNow(); qCDebug(physics) << "EntityMotionState::setWorldTransform()... changed entity:" << _entity->getEntityItemID(); qCDebug(physics) << " last edited:" << _entity->getLastEdited() << formatUsecTime(now - _entity->getLastEdited()) << "ago"; qCDebug(physics) << " last simulated:" << _entity->getLastSimulated() << formatUsecTime(now - _entity->getLastSimulated()) << "ago"; qCDebug(physics) << " last updated:" << _entity->getLastUpdated() << formatUsecTime(now - _entity->getLastUpdated()) << "ago"; #endif }
void EntityMotionState::setWorldTransform(const btTransform &worldTrans) { btQuaternion rot = worldTrans.getRotation(); m_Entity->rotation = rot.getAngle() * (180.f/M_PI) * rot.getAxis().z(); btVector3 pos = worldTrans.getOrigin(); m_Entity->position = pos; m_Transform = worldTrans; }
CIwFMat Physics::GetMatrixFromWheelInfo( const btWheelInfo w, const btRigidBody* b ) { CIwFMat modelMatrix( CIwMat::g_Identity ); const btTransform btTrans = w.m_worldTransform; const btVector3 pos = btTrans.getOrigin(); const float posX = pos.getX(); const float posY = pos.getY(); const float posZ = pos.getZ(); const CIwFVec3 position( posX, posY, posZ ); const btQuaternion q = btTrans.getRotation(); CIwFVec3 rot; QuaternionToEuler( q, rot ); const btQuaternion qBody = b->getOrientation(); CIwFVec3 rotBody; QuaternionToEuler( qBody, rotBody ); CIwFMat mat = CIwFMat::g_Identity; mat.PostRotateX( rot.x ); mat.PostRotateY( -rotBody.y ); // mat.PostRotateY( rot.y ); fix for rotating wheels by y axis, there are glitches after 360 degrees rotating mat.PostRotateZ( -rotBody.z ); modelMatrix.CopyRot( mat ); modelMatrix.SetTrans( position ); return modelMatrix; }
void EntityMotionState::setWorldTransform(const btTransform &worldTrans) { btQuaternion rot = worldTrans.getRotation(); m_entity->setRotation(rot.getAngle() * rot.getAxis().z()); btVector3 pos = worldTrans.getOrigin(); m_entity->setPosition(glm::vec2(pos.x(), pos.y())); m_transform = worldTrans; }
void position_and_orientation_from_transform(glm::vec3& out_position, glm::quat& out_orientation, const btTransform& world_transform) { const btVector3& origin = world_transform.getOrigin(); const btQuaternion& rot = world_transform.getRotation(); out_position = glm::vec3(origin.x(), origin.y(), origin.z()); out_orientation = glm::quat(rot.w(), rot.x(), rot.y(), rot.z()); }
/// btMotionState override. Called when Bullet wants to tell us the body's current transform void setWorldTransform(const btTransform &worldTrans) { /// \todo For a large scene, applying the changed transforms of rigid bodies is slow (slower than the physics simulation itself, /// or handling collisions) due to the large number of signals being fired. // Cannot modify server-authoritative physics object, rather get the transform changes through placeable attributes const bool hasAuthority = rigidBody->HasAuthority(); if (!hasAuthority && !clientExtrapolating) return; if (placeable.Expired()) return; Placeable* p = placeable; // Important: disconnect our own response to attribute changes to not create an endless loop! disconnected = true; AttributeChange::Type changeType = hasAuthority ? AttributeChange::Default : AttributeChange::LocalOnly; // Set transform float3 position = worldTrans.getOrigin(); Quat orientation = worldTrans.getRotation(); // Non-parented case if (p->parentRef.Get().IsEmpty()) { Transform newTrans = p->transform.Get(); newTrans.SetPos(position.x, position.y, position.z); newTrans.SetOrientation(orientation); p->transform.Set(newTrans, changeType); } else // The placeable has a parent itself { Urho3D::Node* parent = p->UrhoSceneNode()->GetParent(); if (parent) { position = parent->WorldToLocal(position); orientation = parent->GetWorldRotation().Inverse() * orientation; Transform newTrans = p->transform.Get(); newTrans.SetPos(position); newTrans.SetOrientation(orientation); p->transform.Set(newTrans, changeType); } } // Set linear & angular velocity if (body) { // Performance optimization: because applying each attribute causes signals to be fired, which is slow in a large scene // (and furthermore, on a server, causes each connection's sync state to be accessed), do not set the linear/angular // velocities if they haven't changed float3 linearVel = body->getLinearVelocity(); float3 angularVel = RadToDeg(body->getAngularVelocity()); if (!linearVel.Equals(rigidBody->linearVelocity.Get())) rigidBody->linearVelocity.Set(linearVel, changeType); if (!angularVel.Equals(rigidBody->angularVelocity.Get())) rigidBody->angularVelocity.Set(angularVel, changeType); } disconnected = false; }
void convertBtTransform2Transform(btTransform bulletTransform, Transform *transform) { btVector3 pos = bulletTransform.getOrigin(); btQuaternion rot = bulletTransform.getRotation(); transform->set_position(pos.getX(), pos.getY(), pos.getZ()); transform->set_rotation(rot.getW(), rot.getX(), rot.getY(), rot.getZ()); }
void Body::Update(float dt, btTransform& Parent) { btVector3 parentPos; parentPos = Parent.getOrigin(); //Get Offset Rotation of a model(child) btQuaternion LocalRot; LocalRot = btQuaternion(m_yaw, m_pitch, m_roll); btQuaternion parentRotation; parentRotation = Parent.getRotation(); //Construct Directx Matricies out of give Quaternion rotations, sets up proper transformation of a model, Global + local XMMATRIX matrixChildRotation = XMMatrixRotationQuaternion(XMLoadFloat4(&XMFLOAT4(LocalRot.getX(), LocalRot.getY(), LocalRot.getZ(), LocalRot.getW()))); XMMATRIX matrixParentRotation = XMMatrixRotationQuaternion(XMLoadFloat4(&XMFLOAT4(parentRotation.getX(), parentRotation.getY(), parentRotation.getZ(), parentRotation.getW()))); //calcualte the final rotation by multiplying quaternions using dxmatrix XMMATRIX matrixFinalRotation = matrixChildRotation * matrixParentRotation; XMFLOAT3 m_offset = XMFLOAT3(0, 0, 0); XMMATRIX childOffset = XMMatrixTranslation(m_offset.x, m_offset.y, m_offset.z); XMMATRIX zeroWorld = XMMatrixTranslation(0, 0, 0); XMMATRIX translationMat = XMMatrixTranslation(parentPos.getX(), parentPos.getY(), parentPos.getZ()); XMMATRIX scaleMat = XMMatrixScaling(m_scale, m_scale, m_scale); XMMATRIX worldMat = childOffset * matrixFinalRotation * zeroWorld * scaleMat * translationMat; XMStoreFloat4x4(&m_worldMat, worldMat); }
void RigidBody::setWorldTransform(const btTransform& worldTrans) { Quaternion newWorldRotation = ToQuaternion(worldTrans.getRotation()); Vector3 newWorldPosition = ToVector3(worldTrans.getOrigin()) - newWorldRotation * centerOfMass_; RigidBody* parentRigidBody = 0; // It is possible that the RigidBody component has been kept alive via a shared pointer, // while its scene node has already been destroyed if (node_) { // If the rigid body is parented to another rigid body, can not set the transform immediately. // In that case store it to PhysicsWorld for delayed assignment Node* parent = node_->GetParent(); if (parent != GetScene() && parent) parentRigidBody = parent->GetComponent<RigidBody>(); if (!parentRigidBody) ApplyWorldTransform(newWorldPosition, newWorldRotation); else { DelayedWorldTransform delayed; delayed.rigidBody_ = this; delayed.parentRigidBody_ = parentRigidBody; delayed.worldPosition_ = newWorldPosition; delayed.worldRotation_ = newWorldRotation; physicsWorld_->AddDelayedWorldTransform(delayed); } MarkNetworkUpdate(); } }
void BulletRigidBodyObject::updateObjectFromBullet(const btTransform& worldTrans) { assert(mFixed == false); LocationInfo& locinfo = mParent->info(mID); btVector3 pos = worldTrans.getOrigin(); btVector3 vel = mObjRigidBody->getLinearVelocity(); TimedMotionVector3f newLocation(mParent->context()->simTime(), MotionVector3f(Vector3f(pos.x(), pos.y(), pos.z()), Vector3f(vel.x(), vel.y(), vel.z()))); mParent->setLocation(mID, newLocation); BULLETLOG(insane, "Updating " << mID << " to velocity " << vel.x() << " " << vel.y() << " " << vel.z()); btQuaternion rot = worldTrans.getRotation(); btVector3 angvel = mObjRigidBody->getAngularVelocity(); Vector3f angvel_siri(angvel.x(), angvel.y(), angvel.z()); float angvel_angle = angvel_siri.normalizeThis(); TimedMotionQuaternion newOrientation( mParent->context()->simTime(), MotionQuaternion( Quaternion(rot.x(), rot.y(), rot.z(), rot.w()), Quaternion(angvel_siri, angvel_angle) ) ); mParent->setOrientation(mID, newOrientation); mParent->addUpdate(mID); }
virtual void setWorldTransform(const btTransform &worldTrans) { if(NULL == mVisibleobj) return; // silently return before we set a node btQuaternion rot = worldTrans.getRotation(); mVisibleobj->setOrientation(rot.w(), rot.x(), rot.y(), rot.z()); btVector3 pos = worldTrans.getOrigin(); mVisibleobj->setPosition(pos.x(), pos.y()+5, pos.z()-5); }
void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const { BT_PROFILE("convexSweepTest"); /// use the broadphase to accelerate the search for objects, based on their aabb /// and for each object with ray-aabb overlap, perform an exact ray test /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical btTransform convexFromTrans,convexToTrans; convexFromTrans = convexFromWorld; convexToTrans = convexToWorld; btVector3 castShapeAabbMin, castShapeAabbMax; /* Compute AABB that encompasses angular movement */ { btVector3 linVel, angVel; btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); btVector3 zeroLinVel; zeroLinVel.setValue(0,0,0); btTransform R; R.setIdentity (); R.setRotation (convexFromTrans.getRotation()); castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); } #ifndef USE_BRUTEFORCE_RAYBROADPHASE btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration); m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax); #else /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) int i; for (i=0;i<m_collisionObjects.size();i++) { btCollisionObject* collisionObject= m_collisionObjects[i]; //only perform raycast if filterMask matches if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); btVector3 collisionObjectAabbMin,collisionObjectAabbMax; collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing btVector3 hitNormal; if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) { objectQuerySingle(castShape, convexFromTrans,convexToTrans, collisionObject, collisionObject->getCollisionShape(), collisionObject->getWorldTransform(), resultCallback, allowedCcdPenetration); } } } #endif //USE_BRUTEFORCE_RAYBROADPHASE }
// This callback is invoked by the physics simulation at the end of each simulation step... // iff the corresponding RigidBody is DYNAMIC and has moved. void EntityMotionState::setWorldTransform(const btTransform& worldTrans) { if (!_entity) { return; } assert(entityTreeIsLocked()); measureBodyAcceleration(); _entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset()); _entity->setRotation(bulletToGLM(worldTrans.getRotation())); _entity->setVelocity(getBodyLinearVelocity()); _entity->setAngularVelocity(getBodyAngularVelocity()); _entity->setLastSimulated(usecTimestampNow()); if (_entity->getSimulatorID().isNull()) { _loopsWithoutOwner++; if (_loopsWithoutOwner > LOOPS_FOR_SIMULATION_ORPHAN && usecTimestampNow() > _nextOwnershipBid) { //qDebug() << "Warning -- claiming something I saw moving." << getName(); setOutgoingPriority(VOLUNTEER_SIMULATION_PRIORITY); } } #ifdef WANT_DEBUG quint64 now = usecTimestampNow(); qCDebug(physics) << "EntityMotionState::setWorldTransform()... changed entity:" << _entity->getEntityItemID(); qCDebug(physics) << " last edited:" << _entity->getLastEdited() << formatUsecTime(now - _entity->getLastEdited()) << "ago"; qCDebug(physics) << " last simulated:" << _entity->getLastSimulated() << formatUsecTime(now - _entity->getLastSimulated()) << "ago"; qCDebug(physics) << " last updated:" << _entity->getLastUpdated() << formatUsecTime(now - _entity->getLastUpdated()) << "ago"; #endif }
void CBaseGameEntity::setGraphicTrans(btTransform trans) { btVector3 v = trans.getOrigin(); m_node->setPosition(v.getX(), v.getY(), v.getZ()); btQuaternion q = trans.getRotation(); m_node->setOrientation(q.w(), q.x(), q.y(), q.z()); }
void MyMotionState::setWorldTransform(const btTransform &xWorldTrans) { if(NULL == mSceneNode) return; // silently return before we set a node btQuaternion xRot = xWorldTrans.getRotation(); mSceneNode->setOrientation(xRot.w(), xRot.x(), xRot.y(), xRot.z()); btVector3 xPos = xWorldTrans.getOrigin(); mSceneNode->setPosition(xPos.x(), xPos.y(), xPos.z()); }
Eigen::Affine3f toEigenTransform(const btTransform& transform) { btVector3 transBullet = transform.getOrigin(); btQuaternion quatBullet = transform.getRotation(); Eigen::Translation3f transEig; transEig = Eigen::Translation3f(transBullet.x(), transBullet.y(), transBullet.z()); Eigen::Matrix3f rotEig = Eigen::Quaternionf(quatBullet.w(),quatBullet.x(),quatBullet.y(),quatBullet.z()).toRotationMatrix(); Eigen::Affine3f out = transEig*rotEig; return out; }
void OgreMotionState::setWorldTransform(const btTransform & worldTrans) { if(mVisibleObj == NULL) return; btQuaternion rot = worldTrans.getRotation(); mVisibleObj->setOrientation(rot.w(), rot.x(), rot.y(), rot.z()); btVector3 pos = worldTrans.getOrigin(); mVisibleObj->setPosition(pos.x(), pos.y(), pos.z()); }
void FreekickMotionState::setWorldTransform(const btTransform &worldTrans) { mPos1 = worldTrans; btQuaternion rot = worldTrans.getRotation(); btVector3 pos = worldTrans.getOrigin(); setOrientation(rot.w(), rot.x(), rot.y(), rot.z()); setPosition(pos.x(), pos.y(), pos.z()); mPhysicsEngine->addUpdatedObject(mId, this); }
void BaseMovementState::setWorldTransform(const btTransform& worldTrans) { //mTransform = worldTrans; btQuaternion rot = worldTrans.getRotation(); Quaternion quat(float(rot.w()), float(rot.x()), float(rot.y()), float(rot.z())); mNode->setOrientation(quat); btVector3 pos = worldTrans.getOrigin(); mNode->setPosition(pos.x(), pos.y(), pos.z()); }
virtual void setWorldTransform(const btTransform &worldTrans) { if(NULL == mVisibleobj) return; // silently return before we SET a node btQuaternion rot = worldTrans.getRotation(); mVisibleobj->setOrientation(rot.w(), rot.x(), rot.y(), rot.z()); btVector3 pos = worldTrans.getOrigin(); // TODO **** XXX need to fix this up such that it renders properly since this doesnt know the scale of the node // also the getCube function returns a cube that isnt centered on Z mVisibleobj->setPosition(pos.x(), pos.y()+5, pos.z()-5); }
//**************************************************************************************************************************tfToPose2D() void PSMpositionNode::tfToPose2D(const btTransform& t, geometry_msgs::Pose2D& pose) { btMatrix3x3 m(t.getRotation()); double roll, pitch, yaw; m.getRPY(roll, pitch, yaw); pose.x = t.getOrigin().getX(); pose.y = t.getOrigin().getY(); pose.theta = yaw; }
void NodeMotionState::setWorldTransform(const btTransform &worldTrans) { const auto &v = worldTrans.getOrigin(); const auto &rot = worldTrans.getRotation(); m_node->transform()->setOrientation(glm::quat(rot.w(), rot.x(), rot.y(), rot.z())); m_node->transform()->setPosition(v.getX(), v.getY(), v.getZ()); m_transform = worldTrans; }
void EntityMotionState::setWorldTransform(const btTransform& transform) { if(owner) { btVector3 pos = transform.getOrigin(); owner->setPosition(glm::vec3(pos.x(), pos.y(), pos.z())); btQuaternion quat = transform.getRotation(); owner->setQuaternionRotation(glm::quat(quat.w(), quat.x(), quat.y(), quat.z())); } else { original_transform = transform; } }
void CFootballPlayer::setGraphicTrans(btTransform trans) { btVector3 v = trans.getOrigin(); btScalar y = v.getY(); m_node->setPosition(v.getX(), y, v.getZ()); m_ringNode->setPosition(0, -y+0.01, 0); btQuaternion q = trans.getRotation(); m_node->setOrientation(q.w(), q.x(), q.y(), q.z()); }
void SubObject::setWorldTransform(const btTransform& transform) { // Get info from bullet const btQuaternion& rotation = transform.getRotation(); const btVector3& position = transform.getOrigin(); // Apply to scene node node_->setOrientation(rotation.w(), rotation.x(), rotation.y(), rotation.z()); node_->setPosition(position.x(), position.y(), position.z()); // Set local info transform_ = transform; }
D3DXMATRIX BT2DX_MATRIX(const btTransform &ms) { btQuaternion q = ms.getRotation(); btVector3 p = ms.getOrigin(); D3DXMATRIX pos, rot, world; D3DXMatrixTranslation(&pos, p.x(), p.y(), p.z()); D3DXMatrixRotationQuaternion(&rot, &BT2DX_QUATERNION(q)); D3DXMatrixMultiply(&world, &rot, &pos); return world; }
CIwFVec3 Physics::GetRotationFromRigidBody( const btRigidBody* b ) { CIwFVec3 rotation; const btTransform btTrans = b->getWorldTransform(); const btQuaternion q = btTrans.getRotation(); QuaternionToEuler( q, rotation ); rotation *= static_cast<float>( BPU_180_PI ); return rotation; }