/* * 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; }