Vector3 Contact::CalculateLocalVelocity(unsigned bodyIndex, marb duration) { RigidBody *thisBody = body[bodyIndex]; // Work out the velocity of the contact point. Vector3 velocity = thisBody->GetRotation() % relativeContactPosition[bodyIndex]; velocity += thisBody->GetVelocity(); // Turn the velocity into contact coordinates Vector3 contactVelocity = contactToWorld.TransformTranspose(velocity); // And return it. return contactVelocity; }
BoundingBox CollisionShape::GetWorldBoundingBox() const { if (shape_ && node_) { // Use the rigid body's world transform if possible, as it may be different from the rendering transform RigidBody* body = GetComponent<RigidBody>(); Matrix3x4 worldTransform = body ? Matrix3x4(body->GetPosition(), body->GetRotation(), node_->GetWorldScale()) : node_->GetWorldTransform(); Vector3 worldPosition(worldTransform * position_); Quaternion worldRotation(worldTransform.Rotation() * rotation_); btTransform shapeWorldTransform(ToBtQuaternion(worldRotation), ToBtVector3(worldPosition)); btVector3 aabbMin, aabbMax; shape_->getAabb(shapeWorldTransform, aabbMin, aabbMax); return BoundingBox(ToVector3(aabbMin), ToVector3(aabbMax)); } else return BoundingBox(); }
void CollisionShape::DrawDebugGeometry(DebugRenderer* debug, bool depthTest) { if (debug && physicsWorld_ && shape_ && node_ && IsEnabledEffective()) { physicsWorld_->SetDebugRenderer(debug); physicsWorld_->SetDebugDepthTest(depthTest); // Use the rigid body's world transform if possible, as it may be different from the rendering transform Matrix3x4 worldTransform; RigidBody* body = GetComponent<RigidBody>(); bool bodyActive = false; if (body) { worldTransform = Matrix3x4(body->GetPosition(), body->GetRotation(), node_->GetWorldScale()); bodyActive = body->IsActive(); } else worldTransform = node_->GetWorldTransform(); Vector3 position = position_; // For terrains, undo the height centering performed automatically by Bullet if (shapeType_ == SHAPE_TERRAIN && geometry_) { HeightfieldData* heightfield = static_cast<HeightfieldData*>(geometry_.Get()); position.y_ += (heightfield->minHeight_ + heightfield->maxHeight_) * 0.5f; } Vector3 worldPosition(worldTransform * position); Quaternion worldRotation(worldTransform.Rotation() * rotation_); btDiscreteDynamicsWorld* world = physicsWorld_->GetWorld(); world->debugDrawObject(btTransform(ToBtQuaternion(worldRotation), ToBtVector3(worldPosition)), shape_, bodyActive ? WHITE : GREEN); physicsWorld_->SetDebugRenderer(0); } }