void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
btCollisionObject* collisionObject,
const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform,
RayResultCallback& resultCallback)
{
if (collisionShape->isSoftBody()) {
btSoftBody* softBody = btSoftBody::upcast(collisionObject);
if (softBody) {
btSoftBody::sRayCast softResult;
if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
{
if (softResult.fraction<= resultCallback.m_closestHitFraction)
{
btCollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = 0;
shapeInfo.m_triangleIndex = softResult.index;
// get the normal
btVector3 normal = softBody->m_faces[softResult.index].m_normal;
btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
if (normal.dot(rayDir) > 0) {
// normal always point toward origin of the ray
normal = -normal;
}
btCollisionWorld::LocalRayResult rayResult
(collisionObject,
&shapeInfo,
normal,
softResult.fraction);
bool normalInWorldSpace = true;
resultCallback.addSingleResult(rayResult,normalInWorldSpace);
}
}
}
}
else {
btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
}
}
void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector)
{
if (m_convexB1)
{
m_simplexSolver->reset();
btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver);
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = transA;
input.m_transformB = transB;
gjk.getClosestPoints(input,pointCollector,0);
} else
{
//convex versus plane
const btConvexShape* convexShape = m_convexA;
const btStaticPlaneShape* planeShape = m_planeShape;
bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
btTransform convexWorldTransform = transA;
btTransform convexInPlaneTrans;
convexInPlaneTrans= transB.inverse() * convexWorldTransform;
btTransform planeInConvex;
planeInConvex= convexWorldTransform.inverse() * transB;
btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;
pointCollector.addContactPoint(
normalOnSurfaceB,
vtxInPlaneWorld,
distance);
}
}
bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger)
{
tr.setIdentity();
{
const char* xyz_str = xml->Attribute("xyz");
if (xyz_str)
{
parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
}
}
{
const char* rpy_str = xml->Attribute("rpy");
if (rpy_str != NULL)
{
btVector3 rpy;
if (parseVector3(rpy,std::string(rpy_str),logger))
{
double phi, the, psi;
double roll = rpy[0];
double pitch = rpy[1];
double yaw = rpy[2];
phi = roll / 2.0;
the = pitch / 2.0;
psi = yaw / 2.0;
btQuaternion orn(
sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
orn.normalize();
tr.setRotation(orn);
}
}
}
return true;
}
bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
const btConvexShape* pConvexA, const btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
class btIDebugDraw* debugDraw)
{
(void)debugDraw;
(void)v;
(void)simplexSolver;
// const btScalar radialmargin(btScalar(0.));
btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin());
btGjkEpaSolver2::sResults results;
if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
pConvexB,transformB,
guessVector,results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
v = results.normal;
return true;
} else
{
if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results))
{
wWitnessOnA = results.witnesses[0];
wWitnessOnB = results.witnesses[1];
v = results.normal;
return false;
}
}
return false;
}
void TransformMotionState::getWorldTransform(btTransform& worldTrans) const {
Matrix4x4 xformMatrix = toMatrix();
float xformMatrixVals[16] = {
xformMatrix[0], xformMatrix[1], xformMatrix[2], xformMatrix[3],
xformMatrix[4], xformMatrix[5], xformMatrix[6], xformMatrix[7],
xformMatrix[8], xformMatrix[9], xformMatrix[10], xformMatrix[11],
xformMatrix[12], xformMatrix[13], xformMatrix[14], xformMatrix[15],
};
worldTrans.setFromOpenGLMatrix(xformMatrixVals);
}
void Wheel::Update(float dt,btTransform & parentTransform)
{
///ROTATION
//get the quaternion rotation from euler angles using bt
//offset rotation(initial rotation)
btQuaternion btLocalRot = btQuaternion(m_yaw, m_pitch, m_roll);
XMMATRIX localRot = XMMatrixRotationQuaternion(XMLoadFloat4(&XMFLOAT4(btLocalRot.getX(), btLocalRot.getY(), btLocalRot.getZ(), btLocalRot.getW())));
//worldspace rotation
btQuaternion btParentRot = parentTransform.getRotation();
XMMATRIX ParentRot = XMMatrixRotationQuaternion(XMLoadFloat4(&XMFLOAT4(btParentRot.getX(), btParentRot.getY(), btParentRot.getZ(), btParentRot.getW())));
//add rotations together
XMMATRIX finalRot = localRot * ParentRot;
btVector3 btparentPos = parentTransform.getOrigin();
XMMATRIX translationMat = XMMatrixTranslation(btparentPos.getX(), btparentPos.getY(), btparentPos.getZ());
//XMMATRIX scaleMat = XMMatrixScaling(m_scale, m_scale, m_scale);
XMMATRIX scaleMat = XMMatrixScaling(m_scale, m_scale, m_scale);
XMMATRIX worldMat = finalRot * scaleMat * translationMat;
XMStoreFloat4x4(&m_worldMat, worldMat);
}
void Matrix4_to_btTransform(JNIEnv * const &jenv, btTransform &target, jobject &source)
{
matrix4_ensurefields(jenv, source);
jfloatArray valArray = (jfloatArray) jenv->GetObjectField(source, matrix4_val);
jfloat * elements = jenv->GetFloatArrayElements(valArray, NULL);
target.setFromOpenGLMatrix(elements);
jenv->ReleaseFloatArrayElements(valArray, elements, JNI_ABORT);
jenv->DeleteLocalRef(valArray);
}
void MotionState::setWorldTransform(const btTransform& trans)
{
if(!rigidbody->IsEnabled()) return;
dvec3 pos = vec3_cast<dvec3>(trans.getOrigin());
quat rot = quat_cast<quat>(trans.getRotation());
// vec3 oldPos = rigidbody->owner->transform->GetWorldPosition();
// quat oldRot = rigidbody->owner->transform->GetRotationQuat();
// if(pos != oldPos)
{
rigidbody->owner->transform->SetWorldPosition(pos);
}
// if(rot != oldRot)
rigidbody->owner->transform->SetRotationQuat(rot);
// rigidbody->body->getWorldTransform().setOrigin(vec3_cast<btVector3>(pos));
// rigidbody->body->getWorldTransform().setRotation(quat_cast<btQuaternion>(rigidbody->owner->transform->GetRotationQuat()));
}
void btTransform_to_Matrix4(JNIEnv * const &jenv, jobject &target, const btTransform &source)
{
matrix4_ensurefields(jenv, target);
jfloatArray valArray = (jfloatArray) jenv->GetObjectField(target, matrix4_val);
jfloat * elements = jenv->GetFloatArrayElements(valArray, NULL);
source.getOpenGLMatrix(elements);
jenv->ReleaseFloatArrayElements(valArray, elements, 0);
jenv->DeleteLocalRef(valArray);
}
virtual void setWorldTransform(const btTransform& tform)
{
auto inv = glm::inverse(m_object->getRotation());
const auto& rot = tform.getRotation();
auto r2 = inv * glm::quat(rot.w(), rot.x(), rot.y(), rot.z());
auto skeleton = m_object->skeleton;
auto& prev = skeleton->getData(m_part->dummy->getIndex()).a;
auto next = prev;
next.rotation = r2;
skeleton->setData(m_part->dummy->getIndex(), { next, prev, true } );
}
// draw sphere
void DebugDrawer::drawSphere(btScalar radius, const btTransform& transform, const btVector3& color, int half)
{
btVector3 p = transform.getOrigin();
const btMatrix3x3& m = transform.getBasis();
const btVector3 x = m * btVector3(radius,0,0);
const btVector3 y = m * btVector3(0,radius,0);
const btVector3 z = m * btVector3(0,0,radius);
const float PI2 = 2*M_PI, ad = M_PI/15.f;
{
btVector3 p1, p1o = p + y, p2o = p + x, p3o = p + y;
for (float a = ad; a <= PI2+ad; a += ad)
{
float s = sinf(a), c = cosf(a);
p1 = p - s*x + c*y; drawLine(p1o, p1, color); p1o = p1;
p1 = p - s*z + c*x; drawLine(p2o, p1, color); p2o = p1;
p1 = p - s*z + c*y; drawLine(p3o, p1, color); p3o = p1;
}
}
}
void CGmObjPhysMan::CMotionState::setWorldTransform( const btTransform &roWorldTransform )
{
btDefaultMotionState::setWorldTransform( roWorldTransform );
if( m_poShape )
{
//const btMatrix3x3 &roBasis = roWorldTransform.getBasis();
//m_poShape->m_oMatrix.Unit();
roWorldTransform.getOpenGLMatrix( m_poShape->m_oMatrix );
//m_poShape->m_oMatrix.Translate( BT2TVEC3( roWorldTransform.getOrigin() ) );
}
}
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)) );
}
}
btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
:m_convexFromTrans(convexFromTrans),
m_convexToTrans(convexToTrans),
m_world(world),
m_resultCallback(resultCallback),
m_allowedCcdPenetration(allowedPenetration),
m_castShape(castShape)
{
btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
btVector3 rayDir = unnormalizedRayDir.normalized();
///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(unnormalizedRayDir);
}
void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTrans, float& unitMeterScaling)
{
upAxisTrans.setIdentity();
unitMeterScaling = 1;
GLInstanceGraphicsShape* shape = 0;
FILE* file = fopen(relativeFileName, "rb");
if (file)
{
int size = 0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
{
b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName);
}
else
{
if (size)
{
//printf("Open DAE file of %d bytes\n",size);
Assimp::Importer importer;
//importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_COLORS);
importer.SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_LINE | aiPrimitiveType_POINT);
// importer.SetPropertyInteger(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1);
aiScene const* scene = importer.ReadFile(relativeFileName,
aiProcess_JoinIdenticalVertices |
//aiProcess_RemoveComponent |
aiProcess_SortByPType |
aiProcess_Triangulate);
if (scene)
{
shape = &visualShapes.expand();
shape->m_scaling[0] = 1;
shape->m_scaling[1] = 1;
shape->m_scaling[2] = 1;
shape->m_scaling[3] = 1;
int index = 0;
shape->m_indices = new b3AlignedObjectArray<int>();
shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
aiMatrix4x4 ident;
addMeshParts(scene, scene->mRootNode, shape, ident);
shape->m_numIndices = shape->m_indices->size();
shape->m_numvertices = shape->m_vertices->size();
ColladaGraphicsInstance& instance = visualShapeInstances.expand();
instance.m_shapeIndex = visualShapes.size() - 1;
}
}
}
}
}
void MyConvex::Render(bool only_wireframe, const btVector3& wire_color) const
{
const float Scale = 1.0f;
glPushMatrix();
btScalar glmat[16]; //4x4 column major matrix for OpenGL.
mTransform.getOpenGLMatrix(glmat);
#ifndef BT_USE_DOUBLE_PRECISION
glMultMatrixf(&(glmat[0]));
#else
glMultMatrixd(&(glmat[0]));
#endif
if(!only_wireframe)
{
btVector3 color(0.0f, 0.5f, 1.0f);
for(int i=0;i<mNbPolys;i++)
{
glNormal3f(mPolys[i].mPlane[0], mPolys[i].mPlane[1], mPolys[i].mPlane[2]);
int NbTris = mPolys[i].mNbVerts-2;
const btVector3& p0 = mVerts[mPolys[i].mIndices[0]]*Scale;
for(int j=1;j<=NbTris;j++)
{
int k = (j+1)%mPolys[i].mNbVerts;
const btVector3& p1 = mVerts[mPolys[i].mIndices[j]]*Scale;
const btVector3& p2 = mVerts[mPolys[i].mIndices[k]]*Scale;
DrawTriangle(p0, p1, p2, color);
}
}
}
{
btVector3 color;
if(only_wireframe)
color = wire_color;
else
color = btVector3(0.0f, 0.0f, 0.0f);
for(int i=0;i<mNbPolys;i++)
{
for(int j=0;j<mPolys[i].mNbVerts;j++)
{
int k = (j+1)%mPolys[i].mNbVerts;
DrawLine(mVerts[mPolys[i].mIndices[j]]*Scale, mVerts[mPolys[i].mIndices[k]]*Scale, color, 1.0f);
}
}
}
glPopMatrix();
}
OSG::Matrix VRPhysics::fromBTTransform(const btTransform t) {
btScalar _m[16];
t.getOpenGLMatrix(_m);
OSG::Matrix m;
for (int i=0;i<4;i++) m[0][i] = _m[i];
for (int i=0;i<4;i++) m[1][i] = _m[4+i];
for (int i=0;i<4;i++) m[2][i] = _m[8+i];
for (int i=0;i<4;i++) m[3][i] = _m[12+i];
//cout << "fromTransform " << m << endl;
return m;
}
D3DXMATRIX BulletPhysics::ConvertBTMatrix( btTransform &trn )
{
//do crazy math shit
btVector3 R = trn.getBasis().getColumn(0);
btVector3 U = trn.getBasis().getColumn(1);
btVector3 L = trn.getBasis().getColumn(2);
btVector3 P = trn.getOrigin();
D3DXVECTOR3 vR, vU, vL, vP;
vR.x = R.x();vR.y = R.y();vR.z = R.z();
vU.x = U.x();vU.y = U.y();vU.z = U.z();
vL.x = L.x();vL.y = L.y();vL.z = L.z();
vP.x = P.x();vP.y = P.y();vP.z = P.z();
D3DXMATRIX matOutput;
matOutput._11 = vR.x;matOutput._12 = vR.y;matOutput._13 = vR.z;matOutput._14 = 0.f;
matOutput._21 = vU.x;matOutput._22 = vU.y;matOutput._23 = vU.z;matOutput._24 = 0.f;
matOutput._31 = vL.x;matOutput._32 = vL.y;matOutput._33 = vL.z;matOutput._34 = 0.f;
matOutput._41 = vP.x;matOutput._42 = vP.y;matOutput._43 = vP.z;matOutput._44 = 1.f;
return matOutput;
}
void BeGraphicsModel::drawToDepth( const btTransform& transform )
{
// btScalar m[16];
transform.getOpenGLMatrix(m_matrix);
m_system->matrixPush(GL_MODELVIEW);
m_system->matrixMult(GL_MODELVIEW, m_matrix);
drawToDepth();
m_system->matrixPop(GL_MODELVIEW);
//glEnable(GL_CULL_FACE);
//glCullFace(GL_BACK);
}
void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr)
{
const btMatrix3x3& rot = tr.getBasis();
const btVector3& r0 = rot[0];
const btVector3& r1 = rot[1];
const btVector3& r2 = rot[2];
const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z();
const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z();
const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z();
out.setValue(x, y, z);
}
// 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();
bool positionSuccess;
_entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset(), positionSuccess, false);
if (!positionSuccess) {
qDebug() << "EntityMotionState::setWorldTransform setPosition failed" << _entity->getID();
}
bool orientationSuccess;
_entity->setOrientation(bulletToGLM(worldTrans.getRotation()), orientationSuccess, false);
if (!orientationSuccess) {
qDebug() << "EntityMotionState::setWorldTransform setOrientation failed" << _entity->getID();
}
_entity->setVelocity(getBodyLinearVelocity());
_entity->setAngularVelocity(getBodyAngularVelocity());
_entity->setLastSimulated(usecTimestampNow());
if (_entity->getSimulatorID().isNull()) {
_loopsWithoutOwner++;
if (_loopsWithoutOwner > LOOPS_FOR_SIMULATION_ORPHAN && usecTimestampNow() > _nextOwnershipBid) {
upgradeOutgoingPriority(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 bmodMotionState::getWorldTransform(btTransform &worldTrans) const {
if(obj->getEntities()->size() == 0) {
worldTrans = btTransform(btQuaternion(0, 0, 0, 1));
return;
}
edict_t * entity = INDEXENT(obj->getEntities()->front());
worldTrans = btTransform(btQuaternion(0, 0, 0, 1), btVector3(entity->v.origin.x, entity->v.origin.y, entity->v.origin.z));
btVector3 angles;
entity->v.angles.CopyToArray(angles.m_floats);
EulerMatrix(angles, worldTrans.getBasis());
}
void CGmObjPhysMan::CMotionState::getWorldTransform( btTransform &roWorldTrans ) const
{
if( m_poShape && m_poShape->m_poBody && m_poShape->m_poBody->IsKinematic() ) // toopt
//if( m_poShape && m_poShape->m_bIsKinematic )
{
//roWorldTrans.setOrigin( TVEC2BT3( m_poShape->GetPos() ) );
roWorldTrans.setFromOpenGLMatrix( m_poShape->m_oMatrix );
}
else
{
btDefaultMotionState::getWorldTransform( roWorldTrans );
}
}
void MotionState::setWorldTransform(const btTransform &transform)
{
lua_rawgeti(L, LUA_REGISTRYINDEX, component_ref);
lua_getfield(L, -1, "transform");
lua_remove(L, -2);
// the stack now contains just the transform component
// set the origin
{
const btVector3 &origin = transform.getOrigin();
l_pushvect(L, origin.x(), origin.y(), origin.z());
lua_setfield(L, -2, "pos");
}
// set the rotation
{
const btQuaternion &rotation = transform.getRotation();
l_pushquaternion(
L, rotation.w(), rotation.x(), rotation.y(), rotation.z());
lua_setfield(L, -2, "orientation");
}
}
void setWorldTransform(const btTransform& worldTrans)
{
const btMatrix3x3& basis = worldTrans.getBasis();
const btVector3& origin = worldTrans.getOrigin();
transform[0][0] = basis[0].x();
transform[0][1] = basis[0].y();
transform[0][2] = basis[0].z();
transform[0][3] = origin.x();
transform[1][0] = basis[1].x();
transform[1][1] = basis[1].y();
transform[1][2] = basis[1].z();
transform[1][3] = origin.y();
transform[2][0] = basis[2].x();
transform[2][1] = basis[2].y();
transform[2][2] = basis[2].z();
transform[2][3] = origin.z();
rbody->getTransformSignal()(transform);
}
void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
{
btAssert(!std::isnan(xform.getOrigin().getX()));
btAssert(!std::isnan(xform.getOrigin().getY()));
btAssert(!std::isnan(xform.getOrigin().getZ()));
btAssert(!std::isnan(xform.getRotation().getX()));
btAssert(!std::isnan(xform.getRotation().getY()));
btAssert(!std::isnan(xform.getRotation().getZ()));
btAssert(!std::isnan(xform.getRotation().getW()));
if (isStaticOrKinematicObject())
{
m_interpolationWorldTransform = m_worldTransform;
} else
{
m_interpolationWorldTransform = xform;
}
m_interpolationLinearVelocity = getLinearVelocity();
m_interpolationAngularVelocity = getAngularVelocity();
m_worldTransform = xform;
updateInertiaTensor();
}
void motion_state::setWorldTransform(const btTransform &worldTrans)
{
if (node == NULL) {
return;
}
float f[16];
worldTrans.getOpenGLMatrix(f);
node->transformation_matrix = glm::mat4( f[0], f[1], f[2], f[3],
f[4], f[5], f[6], f[7],
f[8], f[9], f[10], f[11],
f[12], f[13], f[14], f[15] );
}
void initTable(ColorCloudPtr cloud) {
MatrixXf corners = getTableCornersRansac(cloud);
Vector3f xax = corners.row(1) - corners.row(0);
xax.normalize();
Vector3f yax = corners.row(3) - corners.row(0);
yax.normalize();
Vector3f zax = xax.cross(yax);
float zsgn = (zax(2) > 0) ? 1 : -1;
xax *= - zsgn;
zax *= - zsgn; // so z axis points up
m_axes.col(0) = xax;
m_axes.col(1) = yax;
m_axes.col(2) = zax;
MatrixXf rotCorners = corners * m_axes;
m_mins = rotCorners.colwise().minCoeff();
m_maxes = rotCorners.colwise().maxCoeff();
m_mins(2) = rotCorners(0,2) + LocalConfig::zClipLow;
m_maxes(2) = rotCorners(0,2) + LocalConfig::zClipHigh;
m_transform.setBasis(btMatrix3x3(
xax(0),yax(0),zax(0),
xax(1),yax(1),zax(1),
xax(2),yax(2),zax(2)));
m_transform.setOrigin(btVector3(corners(0,0), corners(0,1), corners(0,2)));
m_poly.points = toROSPoints32(toBulletVectors(corners));
m_inited = true;
}
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling, int clientUpAxis, struct CommonFileIOInterface* fileIO)
{
// GLInstanceGraphicsShape* instance = 0;
//usually COLLADA files don't have that many visual geometries/shapes
visualShapes.reserve(MAX_VISUAL_SHAPES);
float extraScaling = 1; //0.01;
btHashMap<btHashString, int> name2ShapeIndex;
char filename[1024];
if (!fileIO->findResourcePath(relativeFileName, filename, 1024))
{
b3Warning("File not found: %s\n", filename);
return;
}
XMLDocument doc;
//doc.Parse((const char*)filedata, 0, TIXML_ENCODING_UTF8);
b3AlignedObjectArray<char> xmlString;
int fileHandle = fileIO->fileOpen(filename,"r");
if (fileHandle>=0)
{
int size = fileIO->getFileSize(fileHandle);
xmlString.resize(size);
int actual = fileIO->fileRead(fileHandle, &xmlString[0],size);
if (actual==size)
{
}
}
if (xmlString.size()==0)
return;
if (doc.Parse(&xmlString[0], xmlString.size()) != XML_SUCCESS)
//if (doc.LoadFile(filename) != XML_SUCCESS)
return;
//We need units to be in meter, so apply a scaling using the asset/units meter
unitMeterScaling = 1;
upAxisTransform.setIdentity();
//Also we can optionally compensate all transforms using the asset/up_axis as well as unit meter scaling
getUnitMeterScalingAndUpAxisTransform(doc, upAxisTransform, unitMeterScaling, clientUpAxis);
btMatrix4x4 ident;
ident.setIdentity();
readLibraryGeometries(doc, visualShapes, name2ShapeIndex, extraScaling);
readVisualSceneInstanceGeometries(doc, name2ShapeIndex, visualShapeInstances);
}
void setWorldTransform(const btTransform &worldTrans)
{
if(m_irrNode)
{
m_irrNode->setPosition(bullet::bulletVectorToIrrVector(worldTrans.getOrigin()));
m_graphicsWorldTrans = worldTrans * m_centerOfMassOffset ;
}
else
{
assert("Error during motion state updating.");
}
}
