btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
:m_rayFromWorld(rayFromWorld),
m_rayToWorld(rayToWorld),
m_world(world),
m_resultCallback(resultCallback)
{
m_rayFromTrans.setIdentity();
m_rayFromTrans.setOrigin(m_rayFromWorld);
m_rayToTrans.setIdentity();
m_rayToTrans.setOrigin(m_rayToWorld);
btVector3 rayDir = (rayToWorld-rayFromWorld);
rayDir.normalize ();
///what about division by zero? --> just set rayDirection[i] to INF/1e30
m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
m_signs[0] = m_rayDirectionInverse[0] < 0.0;
m_signs[1] = m_rayDirectionInverse[1] < 0.0;
m_signs[2] = m_rayDirectionInverse[2] < 0.0;
m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
}
void ConvertMatrixToBull(const matrix3x4_t& hl, btTransform& transform)
{
Vector forward, left, up, pos;
forward.x = hl[0][0];
forward.y = hl[1][0];
forward.z = hl[2][0];
left.x = hl[0][1];
left.y = hl[1][1];
left.z = hl[2][1];
up.x = hl[0][2];
up.y = hl[1][2];
up.z = hl[2][2];
pos.x = hl[0][3];
pos.y = hl[1][3];
pos.z = hl[2][3];
btVector3 bullForward, bullLeft, bullUp, origin;
ConvertDirectionToBull(forward, bullForward);
ConvertDirectionToBull(-left, bullLeft);
ConvertDirectionToBull(up, bullUp);
ConvertPosToBull(pos, origin);
transform.setBasis(btMatrix3x3(bullForward.x(), bullUp.x(), bullLeft.x(), bullForward.y(), bullUp.y(), bullLeft.y(), bullForward.z(), bullUp.z(), bullLeft.z()));
transform.setOrigin(origin);
}
//**************************************************************************************************************************pose2DtoTf()
void PSMpositionNode::pose2DToTf(const geometry_msgs::Pose2D& pose, btTransform& t)
{
t.setOrigin(btVector3(pose.x, pose.y, 0.0));
btQuaternion q;
q.setRPY(0, 0, pose.theta);
t.setRotation(q);
}
// This callback is invoked by the physics simulation in two cases:
// (1) when the RigidBody is first added to the world
// (irregardless of PhysicsMotionType: STATIC, DYNAMIC, or KINEMATIC)
// (2) at the beginning of each simulation step for KINEMATIC RigidBody's --
// it is an opportunity for outside code to update the object's simulation position
void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
BT_PROFILE("getWorldTransform");
if (!_entity) {
return;
}
assert(entityTreeIsLocked());
if (_motionType == MOTION_TYPE_KINEMATIC) {
BT_PROFILE("kinematicIntegration");
uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
if (hasInternalKinematicChanges()) {
// ACTION_CAN_CONTROL_KINEMATIC_OBJECT_HACK: This kinematic body was moved by an Action
// and doesn't require transform update because the body is authoritative and its transform
// has already been copied out --> do no kinematic integration.
clearInternalKinematicChanges();
_lastKinematicStep = thisStep;
return;
}
// This is physical kinematic motion which steps strictly by the subframe count
// of the physics simulation and uses full gravity for acceleration.
_entity->setAcceleration(_entity->getGravity());
float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
_lastKinematicStep = thisStep;
_entity->stepKinematicMotion(dt);
// and set the acceleration-matches-gravity count high so that if we send an update
// it will use the correct acceleration for remote simulations
_accelerationNearlyGravityCount = (uint8_t)(-1);
}
worldTrans.setOrigin(glmToBullet(getObjectPosition()));
worldTrans.setRotation(glmToBullet(_entity->getWorldOrientation()));
}
//-------------------------------------------------------------------------------------------------------
void Road::updateTriggerPosition(btTransform& trans)
{
trans.setIdentity();
trans.setOrigin(GameState::ogreVecToBullet(mPosition));
Ogre::Quaternion rotation = Ogre::Vector3::UNIT_Z.getRotationTo(mDirection);
trans.setRotation(btQuaternion(rotation.x, rotation.y, rotation.z, rotation.w));
}
void MotionState::getWorldTransform(btTransform &transform) const
{
lua_rawgeti(L, LUA_REGISTRYINDEX, component_ref);
lua_getfield(L, -1, "transform");
lua_remove(L, -2);
// the stack now contains just the transform component
// get the origin
{
lua_getfield(L, -1, "pos");
double x, y, z;
l_tovect(L, -1, &x, &y, &z);
lua_pop(L, 1);
transform.setOrigin(btVector3(x, y, z));
}
// get the rotation
{
lua_getfield(L, -1, "orientation");
double w, x, y, z;
l_toquaternion(L, -1, &w, &x, &y, &z);
lua_pop(L, 1);
transform.setRotation(btQuaternion(x, y, z, w));
}
lua_pop(L, 1);
}
void fbMotionState::getWorldTransform(btTransform& worldTrans) const
{
assert(mVisualObj);
worldTrans.setIdentity();
worldTrans.setRotation(FBToBullet(mVisualObj->GetRot()));
worldTrans.setOrigin(FBToBullet(mVisualObj->GetPos()));
}
// This callback is invoked by the physics simulation in two cases:
// (1) when the RigidBody is first added to the world
// (irregardless of PhysicsMotionType: STATIC, DYNAMIC, or KINEMATIC)
// (2) at the beginning of each simulation step for KINEMATIC RigidBody's --
// it is an opportunity for outside code to update the object's simulation position
void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
BT_PROFILE("getWorldTransform");
if (!_entity) {
return;
}
assert(entityTreeIsLocked());
if (_motionType == MOTION_TYPE_KINEMATIC) {
BT_PROFILE("kinematicIntegration");
// This is physical kinematic motion which steps strictly by the subframe count
// of the physics simulation and uses full gravity for acceleration.
_entity->setAcceleration(_entity->getGravity());
uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
_entity->stepKinematicMotion(dt);
// bypass const-ness so we can remember the step
const_cast<EntityMotionState*>(this)->_lastKinematicStep = thisStep;
// and set the acceleration-matches-gravity count high so that if we send an update
// it will use the correct acceleration for remote simulations
_accelerationNearlyGravityCount = (uint8_t)(-1);
}
worldTrans.setOrigin(glmToBullet(getObjectPosition()));
worldTrans.setRotation(glmToBullet(_entity->getRotation()));
}
virtual void addCamera(_bObj* tmpObject)
{
m_cameraTrans.setOrigin(btVector3(tmpObject->location[IRR_X],tmpObject->location[IRR_Y],tmpObject->location[IRR_Z]));
btMatrix3x3 mat;
mat.setEulerZYX(tmpObject->rotphr[0],tmpObject->rotphr[1],tmpObject->rotphr[2]);
m_cameraTrans.setBasis(mat);
}
void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
{
int n = m_children.size();
btScalar totalMass = 0;
btVector3 center(0, 0, 0);
int k;
for (k = 0; k < n; k++)
{
btAssert(masses[k]>0);
center += m_children[k].m_transform.getOrigin() * masses[k];
totalMass += masses[k];
}
btAssert(totalMass>0);
center /= totalMass;
principal.setOrigin(center);
btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
for ( k = 0; k < n; k++)
{
btVector3 i;
m_children[k].m_childShape->calculateLocalInertia(masses[k], i);
const btTransform& t = m_children[k].m_transform;
btVector3 o = t.getOrigin() - center;
//compute inertia tensor in coordinate system of compound shape
btMatrix3x3 j = t.getBasis().transpose();
j[0] *= i[0];
j[1] *= i[1];
j[2] *= i[2];
j = t.getBasis() * j;
//add inertia tensor
tensor[0] += j[0];
tensor[1] += j[1];
tensor[2] += j[2];
//compute inertia tensor of pointmass at o
btScalar o2 = o.length2();
j[0].setValue(o2, 0, 0);
j[1].setValue(0, o2, 0);
j[2].setValue(0, 0, o2);
j[0] += o * -o.x();
j[1] += o * -o.y();
j[2] += o * -o.z();
//add inertia tensor of pointmass
tensor[0] += masses[k] * j[0];
tensor[1] += masses[k] * j[1];
tensor[2] += masses[k] * j[2];
}
tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
}
static void draw(void)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
float xpos = box1->getCenterOfMassPosition().getX();
float ypos = box1->getCenterOfMassPosition().getY();
float zpos = box1->getCenterOfMassPosition().getZ();
printf("%f %f %f\n",xpos,ypos,zpos);
btTransform trans = box1->getWorldTransform();
trans.setOrigin(btVector3(BoxVel[0], BoxVel[1], BoxVel[2]));
box1->setWorldTransform(trans);
box1->getMotionState()->getWorldTransform(trans);
trans.setOrigin(btVector3(BoxVel[0], BoxVel[1], BoxVel[2]));
box1->getMotionState()->setWorldTransform(trans);
//*** draw box1
glColor3f(0.0, 0.0, 1.0);
glPushMatrix();
box1->getMotionState()->getWorldTransform(trans);
trans.getOpenGLMatrix(matrix);
glMultMatrixf(matrix);
glutSolidCube(40);
glPopMatrix();
//*** draw box2
glColor3f(1.0, 1.0, 0.0);
glPushMatrix();
box2->getMotionState()->getWorldTransform(trans);
trans.getOpenGLMatrix(matrix);
glMultMatrixf(matrix);
glutSolidCube(10);
glPopMatrix();
// draw box 3
glColor3f(0.0,0.0,1.0);
glPushMatrix();
box3->getMotionState()->getWorldTransform(trans);
trans.getOpenGLMatrix(matrix);
glMultMatrixf(matrix);
glutSolidCube(30);
glPopMatrix();
glutSwapBuffers();
}
void getWorldTransform(btTransform& worldTrans) const
{
worldTrans.setBasis( btMatrix3x3(transform[0][0], transform[0][1], transform[0][2],
transform[1][0], transform[1][1], transform[1][2],
transform[2][0], transform[2][1], transform[2][2]) );
worldTrans.setOrigin( btVector3(transform[0][3], transform[1][3], transform[2][3]));
}
virtual void getWorldTransform(btTransform& tform) const
{
const auto& p = m_part->dummy->getDefaultTranslation();
const auto& o = glm::toQuat(m_part->dummy->getDefaultRotation());
tform.setOrigin(btVector3(p.x, p.y, p.z));
tform.setRotation(btQuaternion(o.x, o.y, o.z, o.w));
tform = m_object->collision->getBulletBody()->getWorldTransform() * tform;
}
void gaen_to_bullet_transform(btTransform & bT, const mat43 & gT)
{
bT.setBasis(btMatrix3x3(gT[0][0], gT[1][0], gT[2][0],
gT[0][1], gT[1][1], gT[2][1],
gT[0][2], gT[1][2], gT[2][2]));
bT.setOrigin(btVector3(gT[3][0], gT[3][1], gT[3][2]));
}
// virtual
void AvatarMotionState::getWorldTransform(btTransform& worldTrans) const {
worldTrans.setOrigin(glmToBullet(getObjectPosition()));
worldTrans.setRotation(glmToBullet(getObjectRotation()));
if (_body) {
_body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
_body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
}
}
/// btMotionState override. Called when Bullet wants us to tell the body's initial transform
void getWorldTransform(btTransform &worldTrans) const
{
if (placeable.Expired())
return;
float3 position = placeable->WorldPosition();
Quat orientation = placeable->WorldOrientation();
worldTrans.setOrigin(position);
worldTrans.setRotation(orientation);
}
void EntityMotionState::getWorldTransform(btTransform& transform) const {
if(!owner) {
transform = original_transform;
return;
}
const float* position = owner->getPosition();
transform.setOrigin(btVector3(position[0], position[1], position[2]));
const float* rotation = owner->getQuaternionRotation();
transform.setRotation(btQuaternion(rotation[0], rotation[1], rotation[2], rotation[3]));
}
void DiBone::GetOffsetTransform(btTransform& t) const
{
DiVec3 locScale = GetDerivedScale() * mBindDerivedInverseScale;
DiQuat locRotate = GetDerivedOrientation() * mBindDerivedInverseOrientation;
DiVec3 locTranslate = GetDerivedPosition() +
locRotate * (locScale * mBindDerivedInversePosition);
t.setOrigin(btVector3(locTranslate.x,locTranslate.y,locTranslate.z));
btQuaternion qt;
qt.setValue(locRotate.x,locRotate.y,locRotate.z,locRotate.w);
t.setRotation(qt);
t.setBasis(t.getBasis().scaled(btVector3(locScale.x, locScale.y, locScale.z)));
}
bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
{
jointLowerLimit = 0.f;
jointUpperLimit = 0.f;
if ((*m_data->m_links[urdfLinkIndex]).parent_joint)
{
my_shared_ptr<Joint> pj =(*m_data->m_links[urdfLinkIndex]).parent_joint;
const urdf::Vector3 pos = pj->parent_to_joint_origin_transform.position;
const urdf::Rotation orn = pj->parent_to_joint_origin_transform.rotation;
jointAxisInJointSpace.setValue(pj->axis.x,pj->axis.y,pj->axis.z);
parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));
switch (pj->type)
{
case Joint::REVOLUTE:
jointType = URDFRevoluteJoint;
break;
case Joint::FIXED:
jointType = URDFFixedJoint;
break;
case Joint::PRISMATIC:
jointType = URDFPrismaticJoint;
break;
case Joint::PLANAR:
jointType = URDFPlanarJoint;
break;
case Joint::CONTINUOUS:
jointType = URDFContinuousJoint;
break;
default:
{
printf("Error: unknown joint type %d\n", pj->type);
btAssert(0);
}
};
if (pj->limits)
{
jointLowerLimit = pj->limits.get()->lower;
jointUpperLimit = pj->limits.get()->upper;
}
return true;
} else
{
parent2joint.setIdentity();
return false;
}
}
void RigidBody::getWorldTransform(btTransform& worldTrans) const
{
// We may be in a pathological state where a RigidBody exists without a scene node when this callback is fired,
// so check to be sure
if (node_)
{
lastPosition_ = node_->GetWorldPosition();
lastRotation_ = node_->GetWorldRotation();
worldTrans.setOrigin(ToBtVector3(lastPosition_ + lastRotation_ * centerOfMass_));
worldTrans.setRotation(ToBtQuaternion(lastRotation_));
}
}
/** get the world tansform of the linked position
* @param worldTrans out parameter to palce transform in
* @warning this method is called by physics library and should
* not be called directly
*/
void MotionState::getWorldTransform(btTransform &worldTrans) const
{
// set the location/origin
Point3f& l = this->position->getLocation();
worldTrans.setOrigin (btVector3(l.getX(), l.getY(), l.getZ()));
// set the basis/rotational matrix
// since openGL matrix is column major and Bullet is row
// major, we have to do some converting
btMatrix3x3 matrix;
this->position->getRowMajorRotationMatrix(matrix);
worldTrans.setBasis(matrix);
}
void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback,short int collisionFilterMask)
{
btTransform rayFromTrans,rayToTrans;
rayFromTrans.setIdentity();
rayFromTrans.setOrigin(rayFromWorld);
rayToTrans.setIdentity();
rayToTrans.setOrigin(rayToWorld);
/// go over all objects, and if the ray intersects their 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(collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & collisionFilterMask) {
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
btVector3 hitNormal;
if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
{
rayTestSingle(rayFromTrans,rayToTrans,
collisionObject,
collisionObject->getCollisionShape(),
collisionObject->getWorldTransform(),
resultCallback);
}
}
}
}
// This callback is invoked by the physics simulation in two cases:
// (1) when the RigidBody is first added to the world
// (irregardless of MotionType: STATIC, DYNAMIC, or KINEMATIC)
// (2) at the beginning of each simulation frame for KINEMATIC RigidBody's --
// it is an opportunity for outside code to update the object's simulation position
void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
if (_isKinematic) {
// This is physical kinematic motion which steps strictly by the subframe count
// of the physics simulation.
uint32_t substep = PhysicsEngine::getNumSubsteps();
float dt = (substep - _lastKinematicSubstep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
_entity->simulateKinematicMotion(dt);
_entity->setLastSimulated(usecTimestampNow());
// bypass const-ness so we can remember the substep
const_cast<EntityMotionState*>(this)->_lastKinematicSubstep = substep;
}
worldTrans.setOrigin(glmToBullet(_entity->getPositionInMeters() - ObjectMotionState::getWorldOffset()));
worldTrans.setRotation(glmToBullet(_entity->getRotation()));
}
void ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
if ((*m_data->m_links[linkIndex]).inertial)
{
mass = (*m_data->m_links[linkIndex]).inertial->mass;
localInertiaDiagonal.setValue((*m_data->m_links[linkIndex]).inertial->ixx,(*m_data->m_links[linkIndex]).inertial->iyy,(*m_data->m_links[linkIndex]).inertial->izz);
inertialFrame.setOrigin(btVector3((*m_data->m_links[linkIndex]).inertial->origin.position.x,(*m_data->m_links[linkIndex]).inertial->origin.position.y,(*m_data->m_links[linkIndex]).inertial->origin.position.z));
inertialFrame.setRotation(btQuaternion((*m_data->m_links[linkIndex]).inertial->origin.rotation.x,(*m_data->m_links[linkIndex]).inertial->origin.rotation.y,(*m_data->m_links[linkIndex]).inertial->origin.rotation.z,(*m_data->m_links[linkIndex]).inertial->origin.rotation.w));
} else
{
mass = 1.f;
localInertiaDiagonal.setValue(1,1,1);
inertialFrame.setIdentity();
}
}
// This callback is invoked by the physics simulation in two cases:
// (1) when the RigidBody is first added to the world
// (irregardless of MotionType: STATIC, DYNAMIC, or KINEMATIC)
// (2) at the beginning of each simulation step for KINEMATIC RigidBody's --
// it is an opportunity for outside code to update the object's simulation position
void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
if (!_entity) {
return;
}
assert(entityTreeIsLocked());
if (_motionType == MOTION_TYPE_KINEMATIC) {
// This is physical kinematic motion which steps strictly by the subframe count
// of the physics simulation.
uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
_entity->simulateKinematicMotion(dt);
// bypass const-ness so we can remember the step
const_cast<EntityMotionState*>(this)->_lastKinematicStep = thisStep;
}
worldTrans.setOrigin(glmToBullet(getObjectPosition()));
worldTrans.setRotation(glmToBullet(_entity->getRotation()));
}
void RouteModel::addWaypointGhost(btTransform position)
{
std::shared_ptr<btGhostObject> ghostObject = std::make_shared<btGhostObject>();
position.setOrigin(position.getOrigin());
ghostObject->setWorldTransform(position);
std::shared_ptr<btBoxShape> boxShape = std::make_shared<btBoxShape>(btVector3(40, 2, 2));
ghostObject->setCollisionShape(boxShape.get());
ghostObject->setCollisionFlags(ghostObject->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
ObjectInfo* info = new ObjectInfo(m_routeController, WAYPOINTTYPE);
ghostObject->setUserIndex(WAYPOINTTYPE);
ghostObject->setUserPointer(info); //switching these two somehow results in an error (userpointer points to invalid adress). no clue why..
m_collisionShapes.push_back(boxShape);
m_ghostObjectList.push_back(ghostObject);
}
/** get the world tansform of the linked position
* @param worldTrans out parameter to palce transform in
* @warning this method is called by physics library and should
* not be called directly
*/
void MotionState::getWorldTransform(btTransform &worldTrans) const
{
// set the location/origin
Vector3 l = this->shape._getTranslation();
if (this->position != nullptr)
l += this->position->getLocation();
worldTrans.setOrigin (btVector3(l.x(), l.y(), l.z()));
// set the basis/rotational matrix
// since openGL matrix is column major and Bullet is row
// major, we have to do some converting
Matrix3 matrix;
if (this->position != nullptr)
{
matrix.setColumn(0, this->position->getLocalXAxis());
matrix.setColumn(1, this->position->getUpVector());
matrix.setColumn(2, this->position->getForwardVector());
}
matrix.multiply(this->shape._getRotation());
btMatrix3x3 rot;
rot.setValue(
// fill in x axis, first column
matrix.getColumn(0).x(),
matrix.getColumn(0).y(),
matrix.getColumn(0).z(),
// fill in y axis, second column
matrix.getColumn(1).x(),
matrix.getColumn(1).y(),
matrix.getColumn(1).z(),
// fill in z axis, thrid column
matrix.getColumn(2).x(),
matrix.getColumn(2).y(),
matrix.getColumn(2).z()
);
worldTrans.setBasis(rot);
}
void CityModel::addSpeedZone(btTransform position, int speedLimit)
{
std::shared_ptr<btGhostObject> ghostObject = std::make_shared<btGhostObject>();
position.setOrigin(position.getOrigin());
ghostObject->setWorldTransform(position);
std::shared_ptr<btBoxShape> boxShape = std::make_shared<btBoxShape>(btVector3(50, 2, 2));
ghostObject->setCollisionShape(boxShape.get());
ghostObject->setCollisionFlags(ghostObject->getCollisionFlags() | btCollisionObject::CF_NO_CONTACT_RESPONSE);
ObjectInfo* info = new ObjectInfo(m_levelController, SPEEDZONETYPE);
ghostObject->setUserIndex(SPEEDZONETYPE);
ghostObject->setUserPointer(info); //switching these two somehow results in an error (userpointer points to invalid adress). no clue why..
m_collisionShapes.push_back(boxShape);
m_ghostObjectList.push_back(ghostObject);
m_speedZoneList.push_back({ m_speedZoneList.size(), speedLimit, (btCollisionObject*) ghostObject.get()});
}
void body(int bodytype, vec3 origin,vec3 size, vec3 inertia,float mass) {
btCollisionShape* aShape = NULL;
pos[0] = origin.x; pos[1] = origin.y; pos[2] = origin.z;
inertia.x = 1;
inertia.y = 1;
inertia.z = 1;
mass = 1;
switch (bodytype) {
case BODYTYPE_BOX:
aShape = new btBoxShape(btVector3(btScalar(size.x),btScalar(size.y),btScalar(size.z)));
break;
case BODYTYPE_CYLINDER:
aShape = new btCylinderShape(btVector3(btScalar(size.x),btScalar(size.y),btScalar(size.z)));
break;
case BODYTYPE_SPHERE:
aShape = new btSphereShape(btScalar(size.x));
break;
case BODYTYPE_CAPSULE:
aShape = new btCapsuleShape(btScalar(size.x),btScalar(size.y));
break;
case BODYTYPE_CONE:
aShape = new btConeShape(btScalar(size.x),btScalar(size.y));
break;
}
if (aShape) {
btScalar Mass(mass);
bool isDynamic = (Mass != 0.f);
btVector3 localInertia(inertia.x,inertia.y,inertia.z);
if (isDynamic)
aShape->calculateLocalInertia(mass,localInertia);
transform.setIdentity();
transform.setOrigin(btVector3(origin.x,origin.y,origin.z));
btDefaultMotionState* myMotionState = new btDefaultMotionState(transform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(Mass,myMotionState,aShape,localInertia);
body1 = new btRigidBody(rbInfo);
collisionShapes.push_back(aShape);
dynamicsWorld->addRigidBody(body1);
}
//return body1;
}
void DynamicsMotionState::getWorldTransform(btTransform& transform) const
{
// DALI_LOG_INFO(Debug::Filter::gDynamics, Debug::Verbose, "%s\n", __PRETTY_FUNCTION__);
// get node's world position and rotation
Vector3 position;
Quaternion rotation;
Vector3 axis;
float angle( 0.0f );
mDynamicsBody.GetNodePositionAndRotation(position, rotation);
rotation.ToAxisAngle( axis, angle );
// modify parameters
transform.setIdentity();
transform.setOrigin(btVector3(position.x, position.y, position.z));
if( axis != Vector3::ZERO )
{
transform.setRotation( btQuaternion(btVector3(axis.x, axis.y, axis.z), btScalar(angle)) );
}
}
