void SetTransform(const Transformation& t){
Mat44 mat4;
t.GetHomogeneous(mat4);
auto aT = FBToBullet(mat4);
mSelf->setWorldTransform(aT);
auto numConstraints = mSelf->getNumConstraintRefs();
for (int i = 0; i < numConstraints; ++i){
auto con = mSelf->getConstraintRef(i);
if (con->isEnabled() && con->getConstraintType() == D6_SPRING_2_CONSTRAINT_TYPE){
btFixedConstraint* fixedCon = (btFixedConstraint*)con;
auto a = &con->getRigidBodyA();
auto b = &con->getRigidBodyB();
auto trA = fixedCon->getFrameOffsetA();
auto trB = fixedCon->getFrameOffsetB();
if (b->getUserPointer() == mSelf){
std::swap(a, b);
std::swap(trA, trB);
}
auto bT = aT * trA * trB.inverse();
b->setWorldTransform(bT);
auto ms = b->getMotionState();
if (ms)
ms->setWorldTransform(bT);
}
}
mSelf->activate();
}
void ApplyTorque(const Vec3& torque){
if (mDebug){
Logger::Log(FB_ERROR_LOG_ARG, FormatString("ApplyTorque = %.3f, %.3f, %.3f", torque.x, torque.y, torque.z).c_str());
}
mSelf->applyTorque(FBToBullet(torque));
mSelf->activate();
}
void ApplyImpulse(const Vec3& impulse, const Vec3& rel_pos){
if (mDebug){
Logger::Log(FB_ERROR_LOG_ARG, FormatString("ApplyImpulse = %.3f, %.3f, %.3f", impulse.x, impulse.y, impulse.z).c_str());
}
mSelf->applyImpulse(FBToBullet(impulse), FBToBullet(rel_pos));
mSelf->activate();
}
void SetTransform(const btTransform& aT, VectorMap<void*, int>& set){
if (set.Find(mSelf) != set.end())
return;
mSelf->setWorldTransform(aT);
mSelf->clearForces();
set[mSelf] = 1;
if (mGameFlag != 0)
Logger::Log(FB_DEFAULT_LOG_ARG, FormatString("(info) Setting rigid body transform for %d", mGameFlag).c_str());
auto numConstraints = mSelf->getNumConstraintRefs();
for (int i = 0; i < numConstraints; ++i){
auto con = mSelf->getConstraintRef(i);
auto conType = con->getConstraintType();
if (conType == FIXED_CONSTRAINT_TYPE || conType == D6_SPRING_2_CONSTRAINT_TYPE){
btFixedConstraint* fixedCon = (btFixedConstraint*)con;
auto a = &con->getRigidBodyA();
auto b = &con->getRigidBodyB();
auto trA = fixedCon->getFrameOffsetA();
auto trB = fixedCon->getFrameOffsetB();
if (b->getUserPointer() == mSelf){
std::swap(a, b);
std::swap(trA, trB);
}
auto bT = aT * trA * trB.inverse();
auto rigidBodyImpl = (RigidBodyImpl*)b->getUserPointer();
rigidBodyImpl->mImpl->SetTransform(bT, set);
}
}
mSelf->activate();
}
void ApplyForce(const Vec3& force, const Vec3& rel_pos){
if (mDebug){
Logger::Log(FB_ERROR_LOG_ARG, FormatString("ApplyForce = %.3f, %.3f, %.3f", force.x, force.y, force.z).c_str());
if (force.Length() > 5){
int a = 0;
a++;
}
}
mSelf->applyForce(FBToBullet(force), FBToBullet(rel_pos));
mSelf->activate();
}
void Activate(){
mSelf->activate();
}