/*
* Returns a Quaternion representing the angle between two vectors
*/
kmQuaternion* kmQuaternionBetweenVec3(kmQuaternion* pOut, const kmVec3* u, const kmVec3* v) {
kmVec3 w;
kmScalar len;
kmQuaternion q;
if(kmVec3AreEqual(u, v)) {
kmQuaternionIdentity(pOut);
return pOut;
}
len = sqrtf(kmVec3LengthSq(u) * kmVec3LengthSq(v));
kmVec3Cross(&w, u, v);
kmQuaternionFill(&q, w.x, w.y, w.z, kmVec3Dot(u, v) + len);
return kmQuaternionNormalize(pOut, &q);
}
void PhysicsRigidBody::applyTorqueImpulse(const kmVec3& torque)
{
// If the torque impulse is significant enough, activate the rigid body
// to make sure that it isn't sleeping and apply the torque impulse.
if (kmVec3LengthSq(&torque) > MATH_EPSILON)
{
GP_ASSERT(_body);
_body->activate();
_body->applyTorqueImpulse(BV(torque));
}
}
PhysicsRigidBody::PhysicsRigidBody(Node* node, const PhysicsCollisionShape::Definition& shape, const Parameters& parameters, int group, int mask)
: PhysicsCollisionObject(node, group, mask), _body(NULL), _mass(parameters.mass), _constraints(NULL), _inDestructor(false)
{
GP_ASSERT(Game::getInstance()->getPhysicsController());
GP_ASSERT(_node);
// Create our collision shape.
kmVec3 centerOfMassOffset = vec3Zero;
_collisionShape = Game::getInstance()->getPhysicsController()->createShape(node, shape, ¢erOfMassOffset, parameters.mass != 0.0f);
GP_ASSERT(_collisionShape && _collisionShape->getShape());
// Create motion state object.
_motionState = new PhysicsMotionState(node, this, (kmVec3LengthSq(¢erOfMassOffset) > MATH_EPSILON) ? ¢erOfMassOffset : NULL);
// If the mass is non-zero, then the object is dynamic so we calculate the local
// inertia. However, if the collision shape is a triangle mesh, we don't calculate
// inertia since Bullet doesn't currently support this.
btVector3 localInertia(0.0, 0.0, 0.0);
if (parameters.mass != 0.0)
_collisionShape->getShape()->calculateLocalInertia(parameters.mass, localInertia);
// Create the Bullet physics rigid body object.
btRigidBody::btRigidBodyConstructionInfo rbInfo(parameters.mass, NULL, _collisionShape->getShape(), localInertia);
rbInfo.m_friction = parameters.friction;
rbInfo.m_restitution = parameters.restitution;
rbInfo.m_linearDamping = parameters.linearDamping;
rbInfo.m_angularDamping = parameters.angularDamping;
// Create + assign the new bullet rigid body object.
_body = bullet_new<btRigidBody>(rbInfo);
// Set motion state after rigid body assignment, since bullet will callback on the motion state interface to query
// the initial transform and it will need to access to rigid body (_body).
_body->setMotionState(_motionState);
// Set other initially defined properties.
setKinematic(parameters.kinematic);
setAnisotropicFriction(parameters.anisotropicFriction);
setAngularFactor(parameters.angularFactor);
setLinearFactor(parameters.linearFactor);
// Add ourself to the physics world.
Game::getInstance()->getPhysicsController()->addCollisionObject(this);
if (_collisionShape->getType() == PhysicsCollisionShape::SHAPE_HEIGHTFIELD)
{
// Add a listener on the node's transform so we can track dirty changes to calculate
// an inverse kmMat4 for transforming heightfield points between world and local space.
_node->addListener(this);
}
}
void PhysicsRigidBody::applyForce(const kmVec3& force, const kmVec3* relativePosition)
{
// If the force is significant enough, activate the rigid body
// to make sure that it isn't sleeping and apply the force.
if (kmVec3LengthSq(&force) > MATH_EPSILON)
{
GP_ASSERT(_body);
_body->activate();
if (relativePosition)
_body->applyForce(BV(force), BV(*relativePosition));
else
_body->applyCentralForce(BV(force));
}
}
kmBool kmRay3IntersectTriangle(const kmRay3* ray, const kmVec3* v0, const kmVec3* v1, const kmVec3* v2, kmVec3* intersection, kmVec3* normal, kmScalar* distance) {
kmVec3 e1, e2, pvec, tvec, qvec, dir;
kmScalar det, inv_det, u, v, t;
kmVec3Normalize(&dir, &ray->dir);
kmVec3Subtract(&e1, v1, v0);
kmVec3Subtract(&e2, v2, v0);
kmVec3Cross(&pvec, &dir, &e2);
det = kmVec3Dot(&e1, &pvec);
/* Backfacing, discard. */
if(det < kmEpsilon) {
return KM_FALSE;
}
if(kmAlmostEqual(det, 0)) {
return KM_FALSE;
}
inv_det = 1.0 / det;
kmVec3Subtract(&tvec, &ray->start, v0);
u = inv_det * kmVec3Dot(&tvec, &pvec);
if(u < 0.0 || u > 1.0) {
return KM_FALSE;
}
kmVec3Cross(&qvec, &tvec, &e1);
v = inv_det * kmVec3Dot(&dir, &qvec);
if(v < 0.0 || (u + v) > 1.0) {
return KM_FALSE;
}
t = inv_det * kmVec3Dot(&e2, &qvec);
if(t > kmEpsilon && (t*t) <= kmVec3LengthSq(&ray->dir)) {
kmVec3 scaled;
*distance = t; /* Distance */
kmVec3Cross(normal, &e1, &e2); /* Surface normal of collision */
kmVec3Normalize(normal, normal);
kmVec3Normalize(&scaled, &dir);
kmVec3Scale(&scaled, &scaled, *distance);
kmVec3Add(intersection, &ray->start, &scaled);
return KM_TRUE;
}
return KM_FALSE;
}
kmQuaternion* kmQuaternionRotationBetweenVec3(kmQuaternion* pOut, const kmVec3* vec1, const kmVec3* vec2, const kmVec3* fallback) {
kmVec3 v1, v2;
kmScalar a;
kmVec3Assign(&v1, vec1);
kmVec3Assign(&v2, vec2);
kmVec3Normalize(&v1, &v1);
kmVec3Normalize(&v2, &v2);
a = kmVec3Dot(&v1, &v2);
if (a >= 1.0) {
kmQuaternionIdentity(pOut);
return pOut;
}
if (a < (1e-6f - 1.0f)) {
if (fabs(kmVec3LengthSq(fallback)) < kmEpsilon) {
kmQuaternionRotationAxisAngle(pOut, fallback, kmPI);
} else {
kmVec3 axis;
kmVec3 X;
X.x = 1.0;
X.y = 0.0;
X.z = 0.0;
kmVec3Cross(&axis, &X, vec1);
/*If axis is zero*/
if (fabs(kmVec3LengthSq(&axis)) < kmEpsilon) {
kmVec3 Y;
Y.x = 0.0;
Y.y = 1.0;
Y.z = 0.0;
kmVec3Cross(&axis, &Y, vec1);
}
kmVec3Normalize(&axis, &axis);
kmQuaternionRotationAxisAngle(pOut, &axis, kmPI);
}
} else {
kmScalar s = sqrtf((1+a) * 2);
kmScalar invs = 1 / s;
kmVec3 c;
kmVec3Cross(&c, &v1, &v2);
pOut->x = c.x * invs;
pOut->y = c.y * invs;
pOut->z = c.z * invs;
pOut->w = s * 0.5f;
kmQuaternionNormalize(pOut, pOut);
}
return pOut;
}
