void Physics3DConeTwistConstraint::setFrames(const cocos2d::Mat4& frameA, const cocos2d::Mat4& frameB)
{
const auto& btFrameA = convertMat4TobtTransform(frameA);
const auto& btFrameB = convertMat4TobtTransform(frameB);
static_cast<btConeTwistConstraint*>(_constraint)->setFrames(btFrameA, btFrameB);
}
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));
}
}
}
Physics3DSliderConstraint* Physics3DSliderConstraint::create(Physics3DRigidBody* rbA, Physics3DRigidBody* rbB, const cocos2d::Mat4& frameInA, const cocos2d::Mat4& frameInB ,bool useLinearReferenceFrameA)
{
auto ret = new (std::nothrow) Physics3DSliderConstraint();
ret->_bodyA = rbA;
ret->_bodyB = rbB;
rbA->retain();
rbB->retain();
auto transformA = convertMat4TobtTransform(frameInA);
auto transformB = convertMat4TobtTransform(frameInB);
ret->_constraint = new btSliderConstraint(*rbA->getRigidBody(), *rbB->getRigidBody(), transformA, transformB, useLinearReferenceFrameA);
ret->autorelease();
return ret;
}
Physics3D6DofConstraint* Physics3D6DofConstraint::create(Physics3DRigidBody* rbA, Physics3DRigidBody* rbB, const cocos2d::Mat4& frameInA, const cocos2d::Mat4& frameInB, bool useLinearReferenceFrameA)
{
auto ret = new Physics3D6DofConstraint();
ret->_bodyA = rbA;
ret->_bodyB = rbB;
rbA->retain();
rbB->retain();
auto frameA = convertMat4TobtTransform(frameInA);
auto frameB = convertMat4TobtTransform(frameInB);
ret->_constraint = new btGeneric6DofConstraint(*rbA->getRigidBody(), *rbB->getRigidBody(), frameA, frameB, useLinearReferenceFrameA);
ret->autorelease();
return ret;
}
Physics3DConeTwistConstraint* Physics3DConeTwistConstraint::create(Physics3DRigidBody* rbA, Physics3DRigidBody* rbB, const cocos2d::Mat4& frameA, const cocos2d::Mat4& frameB)
{
auto ret = new (std::nothrow) Physics3DConeTwistConstraint();
ret->_bodyA = rbA;
ret->_bodyB = rbB;
rbA->retain();
rbB->retain();
auto btFrameA = convertMat4TobtTransform(frameA);
auto btFrameB = convertMat4TobtTransform(frameB);
ret->_constraint = new btConeTwistConstraint(*rbA->getRigidBody(), *rbB->getRigidBody(), btFrameA, btFrameB);
ret->autorelease();
return ret;
}
bool Physics3DWorld::sweepShape(Physics3DShape* shape, const cocos2d::Mat4& startTransform, const cocos2d::Mat4& endTransform, Physics3DWorld::HitResult* result)
{
CC_ASSERT(shape->getShapeType() != Physics3DShape::ShapeType::HEIGHT_FIELD && shape->getShapeType() != Physics3DShape::ShapeType::MESH);
auto btStart = convertMat4TobtTransform(startTransform);
auto btEnd = convertMat4TobtTransform(endTransform);
btCollisionWorld::ClosestConvexResultCallback btResult(btStart.getOrigin(), btEnd.getOrigin());
_btPhyiscsWorld->convexSweepTest((btConvexShape*)shape->getbtShape(), btStart, btEnd, btResult);
if (btResult.hasHit())
{
result->hitObj = getPhysicsObject(btResult.m_hitCollisionObject);
result->hitPosition = convertbtVector3ToVec3(btResult.m_hitPointWorld);
result->hitNormal = convertbtVector3ToVec3(btResult.m_hitNormalWorld);
return true;
}
result->hitObj = nullptr;
return false;
}
Physics3DHingeConstraint* Physics3DHingeConstraint::create(Physics3DRigidBody* rbA, const cocos2d::Mat4& rbAFrame, bool useReferenceFrameA)
{
auto ret = new (std::nothrow) Physics3DHingeConstraint();
ret->_constraint = new btHingeConstraint(*rbA->getRigidBody(), convertMat4TobtTransform(rbAFrame), useReferenceFrameA);
ret->_bodyA = rbA;
rbA->retain();
ret->autorelease();
return ret;
}
bool Physics3DShape::initCompoundShape( const std::vector<std::pair<Physics3DShape *, Mat4>> &shapes )
{
_shapeType = ShapeType::COMPOUND;
auto compound = new btCompoundShape;
for (auto iter : shapes){
compound->addChildShape(convertMat4TobtTransform(iter.second), iter.first->getbtShape());
CC_SAFE_RETAIN(iter.first);
_compoundChildShapes.push_back(iter.first);
}
_btShape = compound;
return true;
}
bool Physics3DRigidBody::init(Physics3DRigidBodyDes* info)
{
if (info->shape == nullptr)
return false;
btScalar mass = info->mass;
auto shape = info->shape->getbtShape();
auto localInertia = convertVec3TobtVector3(info->localInertia);
if (mass != 0.f)
{
shape->calculateLocalInertia(mass,localInertia);
}
auto transform = convertMat4TobtTransform(info->originalTransform);
btDefaultMotionState* myMotionState = new btDefaultMotionState(transform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
_btRigidBody = new btRigidBody(rbInfo);
_type = Physics3DObject::PhysicsObjType::RIGID_BODY;
_physics3DShape = info->shape;
_physics3DShape->retain();
if (info->disableSleep)
_btRigidBody->setActivationState(DISABLE_DEACTIVATION);
return true;
}
void Physics3DSliderConstraint::setFrames(const cocos2d::Mat4& frameA, const cocos2d::Mat4& frameB)
{
auto btFrameA = convertMat4TobtTransform(frameA);
auto btFrameB = convertMat4TobtTransform(frameB);
static_cast<btSliderConstraint*>(_constraint)->setFrames(btFrameA, btFrameB);
}
float Physics3DHingeConstraint::getHingeAngle(const cocos2d::Mat4& transA, const cocos2d::Mat4& transB)
{
auto btTransA = convertMat4TobtTransform(transA);
auto btTransB = convertMat4TobtTransform(transB);
return static_cast<btHingeConstraint*>(_constraint)->getHingeAngle(btTransA, btTransB);
}
void Physics3DHingeConstraint::setFrames(const cocos2d::Mat4& frameA, const cocos2d::Mat4& frameB)
{
auto transformA = convertMat4TobtTransform(frameA);
auto transformB = convertMat4TobtTransform(frameB);
static_cast<btHingeConstraint*>(_constraint)->setFrames(transformA, transformB);
}
void Physics3DRigidBody::setCenterOfMassTransform( const cocos2d::Mat4& xform )
{
_btRigidBody->setCenterOfMassTransform(convertMat4TobtTransform(xform));
}
