Math::Matrix4 VisualActor::getModelMatrix(const Math::Vector3d& position, float direction) {
Math::Matrix4 posMatrix;
posMatrix.setPosition(position);
Math::Matrix4 rot1;
rot1.buildAroundX(90);
Math::Matrix4 rot2;
rot2.buildAroundY(270 - direction);
Math::Matrix4 scale;
scale.setValue(2, 2, -1.0f);
return posMatrix * rot1 * rot2 * scale;
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
void
PhysicsActor::getOrientation(Math::Matrix4& _orient)
{
Math::Matrix4 matrix;
NewtonBodyGetMatrix(m_pActor, matrix.m_array);
// ensure a zero offset for the returned orientation matrix:
matrix.setPosition(0.0f, 0.0f, 0.0f);
// transfer values to the return matrix:
for (int i = 0; i < 16; i++)
{
_orient.m_array[i] = matrix.m_array[i];
}
}
Math::Matrix4 VisualProp::getModelMatrix(const Math::Vector3d& position, float direction) {
Math::Matrix4 posMatrix;
posMatrix.setPosition(position);
Math::Matrix4 rot1;
rot1.buildAroundX(90);
Math::Matrix4 rot2;
rot2.buildAroundY(270 - direction);
Math::Matrix4 scale;
scale.setValue(2, 2, -1.0f);
Math::Matrix4 modelTransform = _model->getTransform();
// FIXME: Why has the scale to be after the model transform?
return posMatrix * rot1 * rot2 * modelTransform * scale;
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
void
PhysicsActor::setOrientation(const Math::Matrix4& _orient)
{
// transfer values from input matrix to temporary matrix:
Math::Matrix4 matrix;
for (int i = 0; i < 16; i++)
{
matrix.m_array[i] = _orient.m_array[i];
}
// add offset to orientation matrix before setting body:
Math::Point3 pos;
pos = getPosition();
matrix.setPosition(pos);
setActivationState(true);
m_activationState = 1;
NewtonBodySetMatrix(m_pActor, matrix.m_array);
}
Math::Vector3d Actor::getWorldPos() const {
if (! isAttached())
return getPos();
EMICostume * cost = static_cast<EMICostume *>(_attachedActor->getCurrentCostume());
assert(cost != NULL);
Math::Matrix4 attachedToWorld;
attachedToWorld.setPosition(_attachedActor->getPos());
attachedToWorld.buildFromPitchYawRoll(_attachedActor->getPitch(), _attachedActor->getYaw(), _attachedActor->getRoll());
// If we were attached to a joint, factor in the joint's position & rotation,
// relative to its actor.
if (cost->_emiSkel && cost->_emiSkel->_obj) {
Joint * j = cost->_emiSkel->_obj->getJointNamed(_attachedJoint);
const Math::Matrix4 & jointToAttached = j->_finalMatrix;
attachedToWorld = attachedToWorld * jointToAttached;
}
Math::Vector3d myPos = getPos();
attachedToWorld.transform(&myPos, true);
return myPos;
}