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));
}
Example #3
0
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;
}
Example #4
0
// 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
}
Example #5
0
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;
}
Example #7
0
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;
}
Example #8
0
			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());
			}
Example #9
0
    /// 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;
    }
Example #10
0
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());
}
Example #11
0
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);
}
Example #12
0
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);
 }
Example #15
0
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
}
Example #16
0
// 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());
}
Example #18
0
	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());
}
Example #23
0
 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);
 }
Example #24
0
//**************************************************************************************************************************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;
}
Example #25
0
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;
}
Example #26
0
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());
}
Example #28
0
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;
}
Example #29
0
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;
}