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);
}
/**
* Return the weapon transform for this vehicle
**/
void Helicopter::getWeaponTransform(btTransform &xform)
{
xform = this->getTransform();
btMatrix3x3 down = xform.getBasis() * btMatrix3x3(btQuaternion(0.0f, PI / 2.5f, 0.0f));
xform.setBasis(down);
}
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);
}
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]));
}
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]));
}
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)));
}
/** 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);
}
/** 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 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 MotionState::getWorldTransform(btTransform& worldTrans) const {
const Matrix4 transformation = object().transformationMatrix();
worldTrans.setOrigin(btVector3(transformation.translation()));
worldTrans.setBasis(btMatrix3x3(transformation.rotationScaling()));
}
void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
//todo(erwincoumans)
//the link->m_inertia is NOT necessarily aligned with the inertial frame
//so an additional transform might need to be computed
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
btMatrix3x3 linkInertiaBasis;
btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ;
if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
{
linkMass = 0.f;
principalInertiaX = 0.f;
principalInertiaY = 0.f;
principalInertiaZ = 0.f;
linkInertiaBasis.setIdentity();
}
else
{
linkMass = link->m_inertia.m_mass;
if (link->m_inertia.m_ixy == 0.0 &&
link->m_inertia.m_ixz == 0.0 &&
link->m_inertia.m_iyz == 0.0)
{
principalInertiaX = link->m_inertia.m_ixx;
principalInertiaY = link->m_inertia.m_iyy;
principalInertiaZ = link->m_inertia.m_izz;
linkInertiaBasis.setIdentity();
}
else
{
principalInertiaX = link->m_inertia.m_ixx;
btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
btScalar threshold = 1.0e-6;
int numIterations = 30;
inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations);
principalInertiaX = inertiaTensor[0][0];
principalInertiaY = inertiaTensor[1][1];
principalInertiaZ = inertiaTensor[2][2];
}
}
mass = linkMass;
if (principalInertiaX < 0 ||
principalInertiaX > (principalInertiaY + principalInertiaZ) ||
principalInertiaY < 0 ||
principalInertiaY > (principalInertiaX + principalInertiaZ) ||
principalInertiaZ < 0 ||
principalInertiaZ > (principalInertiaX + principalInertiaY))
{
b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str());
principalInertiaX = 0.f;
principalInertiaY = 0.f;
principalInertiaZ = 0.f;
linkInertiaBasis.setIdentity();
}
localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ);
inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis()*linkInertiaBasis);
}
else
{
mass = 1.f;
localInertiaDiagonal.setValue(1,1,1);
inertialFrame.setIdentity();
}
}
