btRigidBody& btKart::getFixedBody() { static btRigidBody s_fixed(0, 0,0); s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.), btScalar(0.),btScalar(0.))); return s_fixed; }
void btWheelInfo::updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo) { (void)raycastInfo; if (m_raycastInfo.m_isInContact) { btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS ); btVector3 chassis_velocity_at_contactPoint; btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition(); chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos ); btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint ); if ( project >= btScalar(-0.1)) { m_suspensionRelativeVelocity = btScalar(0.0); m_clippedInvContactDotSuspension = btScalar(1.0) / btScalar(0.1); } else { btScalar inv = btScalar(-1.) / project; m_suspensionRelativeVelocity = projVel * inv; m_clippedInvContactDotSuspension = inv; } } else // Not in contact : position wheel in a nice (rest length) position { m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength(); m_suspensionRelativeVelocity = btScalar(0.0); m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS; m_clippedInvContactDotSuspension = btScalar(1.0); } }
btRigidBody* gkDynamicsWorld::getFixedBody() { static btRigidBody s_fixed(0, 0,0); s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); return &s_fixed; }
virtual ~RigidObject() { if(body->getMotionState()) delete body->getMotionState(); world->removeRigidBody(body); delete body; }
// constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2) : btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), m_anchor(anchor), m_axis1(axis1), m_axis2(axis2) { // build frame basis // 6DOF constraint uses Euler angles and to define limits // it is assumed that rotational order is : // Z - first, allowed limits are (-PI,PI); // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number // used to prevent constraint from instability on poles; // new position of X, allowed limits are (-PI,PI); // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs // Build the frame in world coordinate system first btVector3 zAxis = m_axis1.normalize(); btVector3 yAxis = m_axis2.normalize(); btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system btTransform frameInW; frameInW.setIdentity(); frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0], xAxis[1], yAxis[1], zAxis[1], xAxis[2], yAxis[2], zAxis[2]); frameInW.setOrigin(anchor); // now get constraint frame in local coordinate systems m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW; m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW; // sei limits setLinearLowerLimit(btVector3(0., 0., 0.)); setLinearUpperLimit(btVector3(0., 0., 0.)); setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS)); setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI - UNIV_EPS, SIMD_PI - UNIV_EPS)); }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); //start with free m_lowerLimit = btScalar(1e30); m_upperLimit = btScalar(-1e30); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0),m_limit() { // since no frame is given, assume this to be zero angle and just pick rb transform axis // fixed axis in worldspace btVector3 rbAxisA1, rbAxisA2; btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), #ifdef _BT_USE_CENTER_LIMIT_ m_limit(), #endif m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0), m_normalCFM(0), m_normalERP(0), m_stopCFM(0), m_stopERP(0) { m_rbAFrame.getOrigin() = pivotInA; // since no frame is given, assume this to be zero angle and just pick rb transform axis btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); btVector3 rbAxisA2; btScalar projection = axisInA.dot(rbAxisA1); if (projection >= 1.0f - SIMD_EPSILON) { rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else if (projection <= -1.0f + SIMD_EPSILON) { rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else { rbAxisA2 = axisInA.cross(rbAxisA1); rbAxisA1 = rbAxisA2.cross(axisInA); } m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = pivotInB; m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); #ifndef _BT_USE_CENTER_LIMIT_ //start with free m_lowerLimit = btScalar(1.0f); m_upperLimit = btScalar(-1.0f); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; #endif m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); }
void BulletWrapper::utilSyncHeadRepresentation(const EigenTypes::Vector3f& headPosition, float deltaTime) { // apply velocities or apply positions btVector3 target = ToBullet(headPosition); btVector3 current = m_HeadRepresentation->getWorldTransform().getOrigin(); btVector3 targetVelocity = (target - current) / deltaTime; m_HeadRepresentation->setLinearVelocity(targetVelocity); m_HeadRepresentation->setAngularVelocity(btVector3(0, 0, 0)); }
void removePickingConstraint() { if (pickConstraint && dynamicsWorld) { dynamicsWorld->removeConstraint(pickConstraint); delete pickConstraint; pickedBody->forceActivationState(ACTIVE_TAG); pickedBody->setDeactivationTime( 0.f ); } pickConstraint = NULL; pickedBody = NULL; }
virtual ~RigidObject() { if(body) { if(body->getMotionState() != NULL) delete body->getMotionState(); // if(body->getCollisionShape() != NULL) // delete body->getCollisionShape(); world->removeRigidBody(body); delete body; } }
void createBulletObject(Shape shape) { switch (shape) { case ConvexHull : { btConvexHullShape* bcs = new btConvexHullShape(); for (size_t i=0; i<data.size(); i++) { glm::vec3& v = data[i].vertex; bcs->addPoint( glmvec3_to_btVector3(v) ); } btScalar mass(ball_mass); btVector3 localInertia(0,0,0); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects // position and motion btTransform ballTransform; ballTransform.setIdentity(); ballTransform.setOrigin(btVector3(0,ball_initial_height,0)); btDefaultMotionState* bulletMotionState = new btDefaultMotionState(btTransform(ballTransform)); // ball rigidbody info btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,bulletMotionState,bcs,localInertia); // tweak rigidbody info rbInfo.m_restitution = grestitution; rbInfo.m_friction = gfriction; // add ball to the world rigidbody = new btRigidBody(rbInfo); if(!rigidbody) std::cout << "bulletBallBody pointer null" << std::endl; dynamicsWorld->addRigidBody(rigidbody); break; } case TriangleMesh : { // Shape btBvhTriangleMeshShape* boardShape = new btBvhTriangleMeshShape( &bt_triangles, true ); btScalar mass(board_mass); btVector3 localInertia(0,0,0); // transform : default position btTransform boardTransform; boardTransform.setIdentity(); boardTransform.setOrigin(btVector3(0,0,0)); btDefaultMotionState* bulletMotionState = new btDefaultMotionState(btTransform(boardTransform)); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,bulletMotionState,boardShape,localInertia); // tweak rigidbody properties rbInfo.m_restitution = grestitution; rbInfo.m_friction = gfriction; // add to the world rigidbody = new btRigidBody(rbInfo); if(!rigidbody) std::cout << "bulletBoardBody pointer null" << std::endl; rigidbody->setActivationState(DISABLE_DEACTIVATION); dynamicsWorld->addRigidBody(rigidbody); break; } case None : { break; } } // end switch }
bool inside(vec3f pos, float fudge) { mat3f rot; //if(ctr == nullptr) { btTransform trans; rigid_body->getMotionState()->getWorldTransform(trans); btVector3 bpos = trans.getOrigin(); btQuaternion bq = trans.getRotation(); quaternion q = {{bq.x(), bq.y(), bq.z(), bq.w()}}; rot = q.get_rotation_matrix(); vec3f v = {bpos.x(), bpos.y(), bpos.z()}; vec3f rel = pos - v; return within(b, rot.transp() * rel, fudge); } /*rot = ctr->rot_quat.get_rotation_matrix(); vec3f my_pos = xyz_to_vec(ctr->pos); return within(b, rot.transp() * (pos - my_pos));*/ }
void init(objects_container* _ctr, btRigidBody* _rigid_body, bool _can_slide = false) { ctr = _ctr; rigid_body = _rigid_body; //if(ctr) // b = get_bbox(ctr); //else { btVector3 bmin; btVector3 bmax; btTransform none; none.setOrigin(btVector3(0,0,0)); none.setRotation(btQuaternion().getIdentity()); rigid_body->getCollisionShape()->getAabb(none, bmin, bmax); b.min = {bmin.x(), bmin.y(), bmin.z()}; b.max = {bmax.x(), bmax.y(), bmax.z()}; } can_slide = _can_slide; base_diff = base_diff.identity(); }
void updateBall() { if (_ptrBall) // TODO use _motionState ? _rigidBody.getMotionState()->getWorldTransform( _ptrBall->_transform); }
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), m_flags(0), m_useSolveConstraintObsolete(false) { }
Field(const FieldGeometry *f) : ground_ci{{0, 0, 1}, 0.0}, ceil_ci{{0, 0, -1}, -10.0}, left_wall_ci{{1, 0, 0}, -field_limit_x(f)}, right_wall_ci{{-1, 0, 0}, -field_limit_x(f)}, top_wall_ci{{0, 1, 0}, -field_limit_y(f)}, bottom_wall_ci{{0, -1, 0}, -field_limit_y(f)}, goal_ci{f->goal_width, f->goal_depth, f->goal_height, f->goal_wall_width}, ground_body{ground_ci}, ceil_body{ceil_ci}, left_wall_body{left_wall_ci}, right_wall_body{right_wall_ci}, top_wall_body{top_wall_ci}, bottom_wall_body{bottom_wall_ci}, left_goal_body{goal_ci}, right_goal_body{goal_ci} { auto trans1 = btTransform({0, 0, 0, 1}, {-f->field_length / 2, 0, 0}); left_goal_body.setWorldTransform(trans1); auto trans2 = btTransform({0, 0, 1, 0}, {f->field_length / 2, 0, 0}); right_goal_body.setWorldTransform(trans2); }
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), m_angularOnly(false), m_enableAngularMotor(false) { m_rbAFrame.getOrigin() = pivotInA; // since no frame is given, assume this to be zero angle and just pick rb transform axis btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); btVector3 rbAxisA2; btScalar projection = axisInA.dot(rbAxisA1); if (projection >= 1.0f - SIMD_EPSILON) { rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else if (projection <= -1.0f + SIMD_EPSILON) { rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); } else { rbAxisA2 = axisInA.cross(rbAxisA1); rbAxisA1 = rbAxisA2.cross(axisInA); } m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); m_rbBFrame.getOrigin() = pivotInB; m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(), rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(), rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() ); //start with free m_lowerLimit = btScalar(1e30); m_upperLimit = btScalar(-1e30); m_biasFactor = 0.3f; m_relaxationFactor = 1.0f; m_limitSoftness = 0.9f; m_solveLimit = false; }
void move( float x, float z ) { btVector3 temp = btVector3( x*getDT(), 0.0, z*getDT() ); //rigidbody->translate( temp ); rigidbody->applyForce(temp, btVector3(0.0, 0.0, 0.0)); }
PhysicsObject(btDiscreteDynamicsWorld & world, const T & shape, btScalar mass, btVector3 velocity, const btTransform & transform, GameBall * ptrBall): _shape(shape), _motionState(transform), _rigidBody(mass, &_motionState, &_shape), _ptrBall(ptrBall) { // TODO angular velocity ? _rigidBody.setLinearVelocity(velocity); _rigidBody.setDamping(0.1, 0.1); _rigidBody.setRestitution(0.7); _rigidBody.setFriction(0.1); // TODO _rigidBody.setRollingFriction(0.01); world.addRigidBody(&_rigidBody); }
float get_angular_velocity() { btVector3 vel = rigid_body->getAngularVelocity(); vec3f ang = xyzf_to_vec(vel); return ang.length(); }
void glDraw() { if (!model) return; body1->getMotionState()->getWorldTransform(t); t.getOpenGLMatrix(m); glPushMatrix(); glMultMatrixf(m); model->glDraw(); glPopMatrix(); }
void addPickingConstraint(const btVector3& rayFrom, const btVector3& rayTo) { if (!dynamicsWorld) { return; } removePickingConstraint(); if (pickedObjectIndex <= 0 || pickedObjectIndex >= dynamicsWorld->getNumCollisionObjects()) { return; } pickedBody = btRigidBody::upcast(dynamicsWorld->getCollisionObjectArray()[pickedObjectIndex]); btVector3 pickPos = rayTo; btVector3 localPivot = pickedBody->getCenterOfMassTransform().inverse() * pickPos; pickConstraint = new btPoint2PointConstraint(*pickedBody,localPivot); pickedBody->setActivationState(DISABLE_DEACTIVATION); dynamicsWorld->addConstraint(pickConstraint,true); pickingDistance = (rayFrom-rayTo).length(); pickConstraint->m_setting.m_impulseClamp = 3.0f; pickConstraint->m_setting.m_tau = 0.001f; }
void make_dynamic(CommonRigidBodyBase* bullet_scene, float ftime_ms) { if(!is_kinematic) return; float base_time = 1/90.f; float frame_time = ftime_ms / 1000.f; rigid_body->saveKinematicState(base_time); rigid_body->setLinearVelocity(bullet_scene->getBodyAvgVelocity(rigid_body)); rigid_body->setAngularVelocity(bullet_scene->getBodyAvgAngularVelocity(rigid_body)); toggleSaveMotion(); bullet_scene->makeDynamic(rigid_body); is_kinematic = false; }
btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint(btRigidBody& rbB, const btTransform& frameInB, RotateOrder rotOrder) : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB) , m_frameInB(frameInB) , m_rotateOrder(rotOrder) , m_flags(0) { ///not providing rigidbody A means implicitly using worldspace for body A m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB; calculateTransforms(); }
quaternion get_quat() { btTransform newTrans; rigid_body->getMotionState()->getWorldTransform(newTrans); btQuaternion bq = newTrans.getRotation(); return {{bq.x(), bq.y(), bq.z(), bq.w()}}; }
vec3f get_pos() { btTransform newTrans; rigid_body->getMotionState()->getWorldTransform(newTrans); btVector3 bq = newTrans.getOrigin(); return {bq.x(), bq.y(), bq.z()}; }
btTypedConstraint::btTypedConstraint(btTypedConstraintType type) : m_constraintType (type), m_userConstraintType(-1), m_userConstraintId(-1), m_rbA(s_fixed), m_rbB(s_fixed), m_appliedImpulse(btScalar(0.)) { s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); }
CharController(const btVector3 &pos, btCollisionShape *shape, btScalar mass) { // set defaults btVector3 localIntertia = btVector3(1, 0, 1); //shape->calculateLocalInertia(mass, localIntertia); btTransform startTransform = btTransform::getIdentity(); startTransform.setOrigin(pos); btDefaultMotionState *state = new btDefaultMotionState(startTransform); body = new btRigidBody(mass, state, shape, localIntertia); body->setFriction(0.5f); }
void set_trans(cl_float4 clpos, quaternion m) { mat3f mat_diff = base_diff.get_rotation_matrix(); mat3f current_hand = m.get_rotation_matrix(); mat3f my_rot = current_hand * mat_diff; quaternion n; n.load_from_matrix(my_rot); vec3f absolute_pos = {clpos.x, clpos.y, clpos.z}; ///current hand does not take into account the rotation offset when grabbing ///ie we'll double rotate vec3f offset_rot = current_hand * offset; vec3f pos = absolute_pos + offset_rot; btTransform newTrans; //rigid_body->getMotionState()->getWorldTransform(newTrans); newTrans.setOrigin(btVector3(pos.v[0], pos.v[1], pos.v[2])); newTrans.setRotation(btQuaternion(n.x(), n.y(), n.z(), n.w())); rigid_body->getMotionState()->setWorldTransform(newTrans); //rigid_body->setInterpolationWorldTransform(newTrans); //if(ctr) // ctr->set_pos(conv_implicit<cl_float4>(pos)); slide_parent_init = true; slide_saved_parent = absolute_pos; remote_pos = pos; remote_rot = n; kinematic_old = kinematic_current; kinematic_current = xyzf_to_vec(rigid_body->getWorldTransform().getOrigin()); }