void Physics3DComponent::syncNodeToPhysics()
{
if (_physics3DObj->getObjType() == Physics3DObject::PhysicsObjType::RIGID_BODY
|| _physics3DObj->getObjType() == Physics3DObject::PhysicsObjType::COLLIDER)
{
auto mat = _owner->getNodeToWorldTransform();
//remove scale, no scale support for physics
float oneOverLen = 1.f / sqrtf(mat.m[0] * mat.m[0] + mat.m[1] * mat.m[1] + mat.m[2] * mat.m[2]);
mat.m[0] *= oneOverLen;
mat.m[1] *= oneOverLen;
mat.m[2] *= oneOverLen;
oneOverLen = 1.f / sqrtf(mat.m[4] * mat.m[4] + mat.m[5] * mat.m[5] + mat.m[6] * mat.m[6]);
mat.m[4] *= oneOverLen;
mat.m[5] *= oneOverLen;
mat.m[6] *= oneOverLen;
oneOverLen = 1.f / sqrtf(mat.m[8] * mat.m[8] + mat.m[9] * mat.m[9] + mat.m[10] * mat.m[10]);
mat.m[8] *= oneOverLen;
mat.m[9] *= oneOverLen;
mat.m[10] *= oneOverLen;
mat *= _invTransformInPhysics;
if (_physics3DObj->getObjType() == Physics3DObject::PhysicsObjType::RIGID_BODY)
{
auto body = static_cast<Physics3DRigidBody*>(_physics3DObj)->getRigidBody();
auto motionState = body->getMotionState();
motionState->setWorldTransform(convertMat4TobtTransform(mat));
body->setMotionState(motionState);
}
else if (_physics3DObj->getObjType() == Physics3DObject::PhysicsObjType::COLLIDER)
{
auto object = static_cast<Physics3DCollider*>(_physics3DObj)->getGhostObject();
object->setWorldTransform(convertMat4TobtTransform(mat));
}
}
}
void btRigidBody::saveKinematicState(btScalar timeStep)
{
//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
if (timeStep != btScalar(0.))
{
//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
if (getMotionState())
getMotionState()->getWorldTransform(m_worldTransform);
btVector3 linVel,angVel;
btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
m_interpolationLinearVelocity = m_linearVelocity;
m_interpolationAngularVelocity = m_angularVelocity;
m_interpolationWorldTransform = m_worldTransform;
//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
}
}
// protected
void AvatarManager::removeAvatarMotionState(AvatarSharedPointer avatar) {
auto rawPointer = std::static_pointer_cast<Avatar>(avatar);
AvatarMotionState* motionState = rawPointer->getMotionState();
if (motionState) {
// clean up physics stuff
motionState->clearObjectBackPointer();
rawPointer->setMotionState(nullptr);
_avatarMotionStates.remove(motionState);
_motionStatesToAdd.remove(motionState);
_motionStatesToDelete.push_back(motionState);
}
}
CsRigidBody* CsRigidBody::Clone() {
btScalar cloneMass = btScalar(1.0) / getInvMass();
btVector3 cloneInertia(btScalar(1.0)/getInvInertiaDiagLocal().x(), btScalar(1.0)/getInvInertiaDiagLocal().y(), btScalar(1.0) / getInvInertiaDiagLocal().z());
CsMotionState *motionState = NULL;
if (getMotionState()) {
CsMotionState *oldState = (CsMotionState*) getMotionState();
motionState = new CsMotionState(*oldState);
// set this from outer scope
motionState->mObj = NULL;
}
CsRigidBody *cloneBody = new CsRigidBody(cloneMass, motionState, getCollisionShape(), cloneInertia);
return cloneBody;
// see test CCDphysics project
}
void Asteroid::readFromPacket(Packet& p) {
btRigidBody* body = getBody();
btTransform tf;
btVector3 lv, av;
p >> tf >> lv >> av;
body->setWorldTransform(tf);
const btVector3& origin = tf.getOrigin();
const btQuaternion& rot = tf.getRotation();
const btVector3& rotAxis = rot.getAxis();
getNode()->setPosition(origin.x(), origin.y(), origin.z());
getNode()->setOrientation(Ogre::Quaternion(Ogre::Radian(rot.getAngle()), Ogre::Vector3(rotAxis.x(), rotAxis.y(), rotAxis.z())));
getMotionState()->updateTransform(tf);
body->setLinearVelocity(lv);
body->setAngularVelocity(av);
}
PhysicsObject::~PhysicsObject()
{
delete getMotionState();
}
RigidBody::~RigidBody()
{
delete getMotionState();
}