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 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);
}
QDomElement SimDomElement::transformToNode(QDomDocument &doc, const btTransform t)
{
QDomElement trans = doc.createElement( "transform" );
QDomElement origin = doc.createElement( "origin" );
origin.appendChild(vectorToNode(doc, t.getOrigin()));
trans.appendChild(origin);
QDomElement rotate = doc.createElement( "rotation" );
rotate.appendChild(basisToNode(doc, t.getBasis()));
trans.appendChild(rotate);
return trans;
}
ork::CMatrix4 btmtx4toorkmtx4( const btTransform& mtx )
{ ork::CMatrix4 rval;
ork::CVector3 position = ! mtx.getOrigin();
rval.SetTranslation( position );
const btMatrix3x3& mtx33 = mtx.getBasis();
for( int i=0; i<3; i++ )
{ const btVector3& vec = mtx33.getColumn(i);
rval.SetElemXY(i,0,float(vec.x()));
rval.SetElemXY(i,1,float(vec.y()));
rval.SetElemXY(i,2,float(vec.z()));
}
return rval;
}
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;
}
btVector3 tgBulletSpringCableAnchor::getContactNormal() const
{
#ifdef USE_BASIS
const btTransform tr = attachedBody->getWorldTransform();
btVector3 newNormal = (tr.getBasis() * contactNormal);
newNormal = newNormal.length() > 0.0 ? newNormal.normalize() : btVector3(0.0, 0.0, 0.0);
//assert(newNormal.length() == 1.0);
return newNormal;
#else
return contactNormal;
#endif
}
inline
minko::math::mat4
minko::math::fromBulletTransform(const btTransform& transform)
{
auto basis = transform.getBasis();
auto translation = transform.getOrigin();
return minko::math::mat4(
basis[0][0], basis[1][0], basis[2][0], 0.f,
basis[0][1], basis[1][1], basis[2][1], 0.f,
basis[0][2], basis[1][2], basis[2][2], 0.f,
translation[0], translation[1], translation[2], 1.f
);
}
void btTriangleMeshShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
{
btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
btMatrix3x3 abs_b = trans.getBasis().absolute();
btVector3 center = trans(localCenter);
btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
aabbMin = center - extent;
aabbMax = center + extent;
}
void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{
btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5);
btVector3 localOrigin(0, 0, 0);
localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * btScalar(0.5);
localOrigin *= m_localScaling;
btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin();
btVector3 extent = halfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
extent += btVector3(getMargin(),getMargin(),getMargin());
aabbMin = center - extent;
aabbMax = center + extent;
}
void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{
btVector3 halfExtents = getHalfExtents();
btMatrix3x3 abs_b = t.getBasis().absolute();
btPoint3 center = t.getOrigin();
btVector3 extent = btVector3(abs_b[0].dot(halfExtents),
abs_b[1].dot(halfExtents),
abs_b[2].dot(halfExtents));
extent += btVector3(getMargin(),getMargin(),getMargin());
aabbMin = center - extent;
aabbMax = center + extent;
}
void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
{
btVector3 localAxis = dir*trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
min = vtx1.dot(dir);
max = vtx2.dot(dir);
if(min>max)
{
btScalar tmp = min;
min = max;
max = tmp;
}
}
void SimpleDynamicSceneObject::setWorldTransform(const btTransform& trans)
{
#if 1
btVector3 btComOffset(comOffset.getX(), comOffset.getY(), comOffset.getZ());
btComOffset = trans.getBasis() * btComOffset;
btTransform comTrans(trans.getRotation(), trans.getOrigin() + btComOffset);
#else
btTransform comTrans(trans);
#endif
// +com
RigidBodySceneObject::setWorldTransform(comTrans);
}
/**\fn void print_btTransform(btTransform xf)
* \brief print a btTransform object
* \param xf - a btTransform object to print
* \return void
* \ingroup Kinematics
*/
void print_btTransform(btTransform xf)
{
btMatrix3x3 rr = xf.getBasis();
btVector3 vv = xf.getOrigin();
std::stringstream ss;
cout << fixed;
for (int i=0; i<3; i++)
{
for (int j=0; j<3; j++)
ss << rr[i][j] << "\t ";
ss << vv[i] << endl;
}
ss << "0\t 0\t 0\t 1\n";
log_msg("\n%s", ss.str().c_str());
}
inline mat4x4 fromBt(const btTransform& a)
{
mat4x4 r;
const btMatrix3x3& basis = a.getBasis();
for(int i = 0; i < 3; ++i)
{
for(int j = 0; j < 3; ++j)
r(j, i) = basis[i][j];
r(3, i) = 0;
}
const btVector3& origin = a.getOrigin();
r(0, 3) = origin.x();
r(1, 3) = origin.y();
r(2, 3) = origin.z();
r(3, 3) = 1;
return r;
}
virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex )
{
btCollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = partId;
shapeInfo.m_triangleIndex = triangleIndex;
btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal;
btCollisionWorld::LocalRayResult rayResult
(m_collisionObject,
&shapeInfo,
hitNormalWorld,
hitFraction);
bool normalInWorldSpace = true;
return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace);
}
void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{
btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5);
halfExtents += btVector3(getMargin(),getMargin(),getMargin());
btMatrix3x3 abs_b = t.getBasis().absolute();
btPoint3 center = t.getOrigin();
btVector3 extent = btVector3(abs_b[0].dot(halfExtents),
abs_b[1].dot(halfExtents),
abs_b[2].dot(halfExtents));
aabbMin = center - extent;
aabbMax = center + extent;
}
inline
minko::math::Matrix4x4::Ptr
minko::math::fromBulletTransform(const btTransform& transform,
minko::math::Matrix4x4::Ptr output)
{
auto basis = transform.getBasis();
auto translation = transform.getOrigin();
if (output == nullptr)
output = Matrix4x4::create();
return output->initialize(
basis[0][0], basis[0][1], basis[0][2], translation[0],
basis[1][0], basis[1][1], basis[1][2], translation[1],
basis[2][0], basis[2][1], basis[2][2], translation[2],
0.0f, 0.0f, 0.0f, 1.0f
);
}
static void draw(btTransform trans)
{
//temp->
glColor3f(1.0f, 1.0f, 1.0f);
glPushMatrix();
//glScaled(0.3f, 0.3f, 1);
glTranslatef(trans.getOrigin().getX(), trans.getOrigin().getY(), 0);
btScalar yaw, pitch, roll;
trans.getBasis().getEulerYPR(yaw, pitch, roll);
glRotatef(yaw*180/3.14159265358979f, 0,0,1);
glBegin(GL_QUADS);
glVertex2f(-10, -10);
glVertex2f(10, -10);
glVertex2f(10, 10);
glVertex2f(-10, 10);
glEnd();
glPopMatrix();
}
/** set the world transform of the linked position
* @param worldTrans the transform to set the position to
* @warning this method is called by physics library and should
* not be called directly
*/
void MotionState::setWorldTransform (const btTransform &worldTrans)
{
// static objects don't move
if (this->position == nullptr)
return;
const btVector3& location = worldTrans.getOrigin();
const btMatrix3x3& matrix = worldTrans.getBasis();
btVector3 upV = matrix.getColumn(1);
btVector3 forwardV = matrix.getColumn(2);
// TODO: clean this up, no need to use
(*this->position) = Position(
Vector3(location.getX(), location.getY(), location.getZ()),
Vector3(forwardV.getX(), forwardV.getY(), forwardV.getZ()),
Vector3(upV.getX(), upV.getY(), upV.getZ())
);
}
void btBarrelShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{
btVector3 halfExtents;
halfExtents.setValue((R_hor+R_offset),
(R_vert),
(R_hor+R_offset));
btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin();
btVector3 extent = btVector3(abs_b[0].dot(halfExtents),
abs_b[1].dot(halfExtents),
abs_b[2].dot(halfExtents));
extent += btVector3(getMargin(),getMargin(),getMargin());
aabbMin = center - extent;
aabbMax = center + extent;
}
bool TrackDisplay::transform(const btTransform &pose, const btVector3 &scaleIn,
Ogre::Vector3& pos, Ogre::Quaternion& orient, Ogre::Vector3& scaleOut) {
pos = Ogre::Vector3(pose.getOrigin().x(), pose.getOrigin().y(),
pose.getOrigin().z());
rviz::robotToOgre(pos);
btQuaternion quat;
pose.getBasis().getRotation(quat);
orient = Ogre::Quaternion::IDENTITY;
rviz::ogreToRobot(orient);
orient = Ogre::Quaternion(quat.w(), quat.x(), quat.y(), quat.z()) * orient;
rviz::robotToOgre(orient);
scaleOut = Ogre::Vector3(scaleIn.x(), scaleIn.y(), scaleIn.z());
rviz::scaleRobotToOgre(scaleOut);
return true;
}
void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const
{
#if 1
min = FLT_MAX;
max = -FLT_MAX;
btVector3 witnesPtMin;
btVector3 witnesPtMax;
int numVerts = m_unscaledPoints.size();
for(int i=0;i<numVerts;i++)
{
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
btVector3 pt = trans * vtx;
btScalar dp = pt.dot(dir);
if(dp < min)
{
min = dp;
witnesPtMin = pt;
}
if(dp > max)
{
max = dp;
witnesPtMax=pt;
}
}
#else
btVector3 localAxis = dir*trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
min = vtx1.dot(dir);
max = vtx2.dot(dir);
#endif
if(min>max)
{
btScalar tmp = min;
min = max;
max = tmp;
}
}
glm::vec3
BodyParticle::modelTransform(unsigned int a_index, const btTransform &a_transform) const{
const btMatrix3x3& basis(a_transform.getBasis());
const btVector3& row_x(basis[0]);
const btVector3& row_y(basis[1]);
const btVector3& row_z(basis[2]);
const btVector3& origin(a_transform.getOrigin());
glm::vec3 worldPos(0.0);
const glm::vec3& objectPos(m_particles[a_index]);
worldPos.x = row_x.getX() * objectPos.x + row_x.getY() * objectPos.y + row_x.getZ() * objectPos.z;
worldPos.y = row_y.getX() * objectPos.x + row_y.getY() * objectPos.y + row_y.getZ() * objectPos.z;
worldPos.z = row_z.getX() * objectPos.x + row_z.getY() * objectPos.y + row_z.getZ() * objectPos.z;
worldPos.x += origin.getX();
worldPos.y += origin.getY();
worldPos.z += origin.getZ();
return worldPos;
}
TransformMatrix TransformMatrix::fromBullet(btTransform trans)
{
auto matrix = TransformMatrix::identity();
auto origin = trans.getOrigin();
auto base = trans.getBasis();
// Position
matrix.values[0][3] = origin.getX()*1000.0;
matrix.values[1][3] = origin.getY()*1000.0;
matrix.values[2][3] = origin.getZ()*1000.0;
// Rotation
for (int x=0; x<3; x++) {
for (int y=0; y<3; y++) {
matrix.values[x][y] = base[x][y];
}
}
return matrix;
}
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);
}
}
void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{
btVector3 separatingNormal = separatingNormal1.normalized();
const btVector3 c0 = transA * hullA.m_localCenter;
const btVector3 c1 = transB * hullB.m_localCenter;
const btVector3 DeltaC2 = c0 - c1;
btScalar curMaxDist=maxDist;
int closestFaceB=-1;
btScalar dmax = -FLT_MAX;
{
for(int face=0;face<hullB.m_faces.size();face++)
{
const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
const btVector3 WorldNormal = transB.getBasis() * Normal;
btScalar d = WorldNormal.dot(separatingNormal);
if (d > dmax)
{
dmax = d;
closestFaceB = face;
}
}
}
btVertexArray worldVertsB1;
{
const btFace& polyB = hullB.m_faces[closestFaceB];
const int numVertices = polyB.m_indices.size();
for(int e0=0;e0<numVertices;e0++)
{
const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
worldVertsB1.push_back(transB*b);
}
}
if (closestFaceB>=0)
clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);
}
/* Returns the support point of the minkowski sum:
* MSUM(Pellet, ConvexShape)
*
*/
void supportPoints (const btTransform xformRay,
const btTransform xformB,
const int shapeType,
const void* shape,
SpuConvexPolyhedronVertexData* convexVertexData,
const btScalar marginB,
const btVector3& seperatingAxis,
btVector3& w,
btVector3& supVertexRay,
btVector3& supVertexB)
{
btVector3 saUnit = seperatingAxis;
saUnit.normalize();
btVector3 SupportPellet = xformRay(0.0001 * -saUnit);
btVector3 rotatedSeperatingAxis = seperatingAxis * xformB.getBasis();
btVector3 SupportShape = xformB(localGetSupportingVertexWithoutMargin(shapeType, (void*)shape, rotatedSeperatingAxis, convexVertexData));
SupportShape += saUnit * marginB;
w = SupportPellet - SupportShape;
supVertexRay = SupportPellet;
supVertexB = SupportShape;
}
// 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 btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const
{
#if 1
minProj = FLT_MAX;
maxProj = -FLT_MAX;
int numVerts = m_unscaledPoints.size();
for(int i=0;i<numVerts;i++)
{
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
btVector3 pt = trans * vtx;
btScalar dp = pt.dot(dir);
if(dp < minProj)
{
minProj = dp;
witnesPtMin = pt;
}
if(dp > maxProj)
{
maxProj = dp;
witnesPtMax=pt;
}
}
#else
btVector3 localAxis = dir*trans.getBasis();
witnesPtMin = trans(localGetSupportingVertex(localAxis));
witnesPtMax = trans(localGetSupportingVertex(-localAxis));
minProj = witnesPtMin.dot(dir);
maxProj = witnesPtMax.dot(dir);
#endif
if(minProj>maxProj)
{
btSwap(minProj,maxProj);
btSwap(witnesPtMin,witnesPtMax);
}
}
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);
}
