void OpenGLRenderer::setupCameraPerspective(float pitch, float heading, float fov) {
// TODO: Find a correct and exact formula for the FOV
GLfloat glFOV = 0.63 * fov; // Approximative and experimental formula
if (fov > 79.0 && fov < 81.0)
glFOV = 50.5; // Somewhat good value for fov == 80
else if (fov > 59.0 && fov < 61.0)
glFOV = 36.0; // Somewhat good value for fov == 60
Common::Rect frame = frameViewport();
glViewport(frame.left, frame.top, frame.width(), frame.height());
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
Math::Matrix4 m = Math::makePerspectiveMatrix(glFOV, (GLfloat)kOriginalWidth / (GLfloat)kFrameHeight, 1.0, 10000.0);
glMultMatrixf(m.getData());
// Rotate the model to simulate the rotation of the camera
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glRotatef(pitch, -1.0f, 0.0f, 0.0f);
glRotatef(heading - 180.0f, 0.0f, 1.0f, 0.0f);
glGetDoublev(GL_MODELVIEW_MATRIX, _cubeModelViewMatrix);
glGetDoublev(GL_PROJECTION_MATRIX, _cubeProjectionMatrix);
glGetIntegerv(GL_VIEWPORT, (GLint *)_cubeViewport);
}
void Lua_V2::WorldToScreen() {
lua_Object xObj = lua_getparam(1);
lua_Object yObj = lua_getparam(2);
lua_Object zObj = lua_getparam(3);
if (!lua_isnumber(xObj) || !lua_isnumber(yObj) || !lua_isnumber(zObj)) {
lua_pushnumber(0.0);
lua_pushnumber(0.0);
return;
}
float x = lua_getnumber(xObj);
float y = lua_getnumber(yObj);
float z = lua_getnumber(zObj);
Math::Vector3d pos = Math::Vector3d(x, y, z);
const Set::Setup *setup = g_emi->getCurrSet()->getCurrSetup();
const Math::Vector3d interest = setup->_interest;
const float roll = setup->_roll;
const Math::Quaternion quat = Math::Quaternion(interest.x(), interest.y(), interest.z(), roll);
Math::Matrix4 view = quat.toMatrix();
view.transpose();
pos -= setup->_pos;
pos = view.getRotation() * pos;
pos.z() = -pos.z();
Math::Matrix4 proj = GfxBase::makeProjMatrix(setup->_fov, setup->_nclip, setup->_fclip);
proj.transpose();
Math::Vector4d screen = proj * Math::Vector4d(pos.x(), pos.y(), pos.z(), 1.0);
screen /= screen.w();
lua_pushnumber((screen.x() + 1) * 320);
lua_pushnumber((1 - screen.y()) * 240);
}
void EMIModel::setSkeleton(Skeleton *skel) {
if (_skeleton == skel) {
return;
}
_skeleton = skel;
if (!skel || !_numBoneInfos) {
return;
}
int boneVert = 0;
delete[] _vertexBoneInfo; _vertexBoneInfo = NULL;
delete[] _vertexBone; _vertexBone = NULL;
_vertexBoneInfo = new int[_numBoneInfos];
_vertexBone = new int[_numBoneInfos]; // Oversized, but yeah.
for (int i = 0; i < _numBoneInfos; i++) {
_vertexBoneInfo[i] = _skeleton->findJointIndex(_boneNames[_boneInfos[i]._joint], _skeleton->_numJoints);
if (_boneInfos[i]._incFac == 1) {
_vertexBone[boneVert] = i;
boneVert++;
}
}
Math::Vector3d vertex;
Math::Matrix4 mat;
for (int i = 0; i < _numVertices; i++) {
vertex = _vertices[i];
if (_vertexBoneInfo[_vertexBone[i]] != -1) {
mat = _skeleton->_joints[_vertexBoneInfo[_vertexBone[i]]]._absMatrix;
mat.inverseTranslate(&vertex);
mat.inverseRotate(&vertex);
}
_vertices[i] = vertex;
}
}
void SoundTrack::updatePosition() {
if (!_positioned)
return;
Set *set = g_grim->getCurrSet();
Set::Setup *setup = set->getCurrSetup();
Math::Vector3d cameraPos = setup->_pos;
Math::Vector3d vector = _pos - cameraPos;
float distance = vector.getMagnitude();
_attenuation = MAX(0.0f, 1.0f - distance / (_volume * 100.0f / Audio::Mixer::kMaxChannelVolume));
if (!isfinite(_attenuation)) {
_attenuation = 0.0f;
}
Math::Matrix4 worldRot = setup->_rot;
Math::Vector3d relPos = (_pos - setup->_pos);
Math::Vector3d p(relPos);
worldRot.inverseRotate(&p);
float angle = atan2(p.x(), p.z());
float pan = sin(angle);
_balance = (int)(pan * 127.0f);
if (_handle) {
g_system->getMixer()->setChannelBalance(*_handle, _balance);
g_system->getMixer()->setChannelVolume(*_handle, (byte)getEffectiveVolume());
}
}
void TinyGLRenderer::setupCameraPerspective(float pitch, float heading, float fov) {
// TODO: Find a correct and exact formula for the FOV
TGLfloat glFOV = 0.63 * fov; // Approximative and experimental formula
if (fov > 79.0 && fov < 81.0)
glFOV = 50.5; // Somewhat good value for fov == 80
else if (fov > 59.0 && fov < 61.0)
glFOV = 36.0; // Somewhat good value for fov == 60
// NOTE: tinyGL viewport implementation needs to be checked as it doesn't behave the same as openGL
tglViewport(0, kTopBorderHeight, kOriginalWidth, kFrameHeight);
tglMatrixMode(TGL_PROJECTION);
tglLoadIdentity();
Math::Matrix4 m = Math::makePerspectiveMatrix(glFOV, (TGLfloat)kOriginalWidth / (TGLfloat)kFrameHeight, 1.0, 10000.0);
tglMultMatrixf(m.getData());
// Rotate the model to simulate the rotation of the camera
tglMatrixMode(TGL_MODELVIEW);
tglLoadIdentity();
tglRotatef(pitch, -1.0f, 0.0f, 0.0f);
tglRotatef(heading - 180.0f, 0.0f, 1.0f, 0.0f);
tglGetFloatv(TGL_MODELVIEW_MATRIX, _cubeModelViewMatrix);
tglGetFloatv(TGL_PROJECTION_MATRIX, _cubeProjectionMatrix);
tglGetIntegerv(TGL_VIEWPORT, (TGLint *)_cubeViewport);
}
inline void tglMultMatrix(const math::Matrix4& matrix)
{
#if defined(TAU_SCALAR_DOUBLE)
glMultTransposeMatrixd((const double*)matrix.getData());
#else
glMultTransposeMatrixf((const float*)matrix.getData());
#endif
}
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
const Math::Point3
PhysicsActor::getPosition()
{
Math::Point3 position;
Math::Matrix4 matrix;
NewtonBodyGetMatrix(m_pActor, matrix.m_array);
matrix.getPosition(position);
return position;
}
bool VisualActor::intersectRay(const Math::Ray &ray, const Math::Vector3d position, float direction) {
Math::Matrix4 inverseModelMatrix = getModelMatrix(position, direction);
inverseModelMatrix.inverse();
// Build an object local ray from the world ray
Math::Ray localRay = ray;
localRay.transform(inverseModelMatrix);
return _model->intersectRay(localRay);
}
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];
}
}
void BaseRenderer::setupCameraPerspective(float pitch, float heading, float fov) {
_projectionMatrix = makeProjectionMatrix(fov);
_modelViewMatrix = Math::Matrix4(180.0f - heading, pitch, 0.0f, Math::EO_YXZ);
Math::Matrix4 proj = _projectionMatrix;
Math::Matrix4 model = _modelViewMatrix;
proj.transpose();
model.transpose();
_mvpMatrix = proj * model;
_frustum.setup(_mvpMatrix);
_mvpMatrix.transpose();
}
void OpenGLSPropRenderer::render(const Math::Vector3d position, float direction) {
if (_faceVBO == -1) {
// Update the OpenGL Buffer Objects if required
clearVertices();
uploadVertices();
}
_gfx->set3DMode();
Math::Matrix4 model = getModelMatrix(position, direction);
Math::Matrix4 view = StarkScene->getViewMatrix();
Math::Matrix4 projection = StarkScene->getProjectionMatrix();
Math::Matrix4 mvp = projection * view * model;
mvp.transpose();
_shader->use(true);
_shader->setUniform("mvp", mvp);
const Common::Array<Formats::BiffMesh::Face> &faces = _model->getFaces();
const Common::Array<Formats::BiffMesh::Material> &materials = _model->getMaterials();
for (Common::Array<Formats::BiffMesh::Face>::const_iterator face = faces.begin(); face != faces.end(); ++face) {
const Formats::BiffMesh::Material &material = materials[face->materialId];
// For each face draw its vertices from the VBO, indexed by the EBO
const Gfx::Texture *tex = _texture->getTexture(material.texture);
if (tex) {
tex->bind();
} else {
glBindTexture(GL_TEXTURE_2D, 0);
}
GLuint ebo = _faceEBO[face];
_shader->enableVertexAttribute("position", _faceVBO, 3, GL_FLOAT, GL_FALSE, 9 * sizeof(float), 0);
_shader->enableVertexAttribute("normal", _faceVBO, 3, GL_FLOAT, GL_FALSE, 9 * sizeof(float), 12);
_shader->enableVertexAttribute("texcoord", _faceVBO, 3, GL_FLOAT, GL_FALSE, 9 * sizeof(float), 24);
_shader->use(true);
_shader->setUniform("textured", tex != nullptr);
_shader->setUniform("color", Math::Vector3d(material.r, material.g, material.b));
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ebo);
glDrawElements(GL_TRIANGLES, face->vertexIndices.size(), GL_UNSIGNED_INT, 0);
glUseProgram(0);
}
}
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);
}
void ShaderRenderer::setupCameraPerspective(float pitch, float heading, float fov) {
// TODO: Find a correct and exact formula for the FOV
GLfloat glFOV = 0.63 * fov; // Approximative and experimental formula
if (fov > 79.0 && fov < 81.0)
glFOV = 50.5; // Somewhat good value for fov == 80
else if (fov > 59.0 && fov < 61.0)
glFOV = 36.0; // Somewhat good value for fov == 60
glViewport(0, kBottomBorderHeight, kOriginalWidth, kFrameHeight);
const Math::Vector2d topLeft = Math::Vector2d(0, kBottomBorderHeight + kFrameHeight);
const Math::Vector2d bottomRight = Math::Vector2d(kOriginalWidth, kBottomBorderHeight);
_viewport = Math::Rect2d(topLeft, bottomRight);
float nclip = 1.0, fclip = 10000.0;
float aspect = _viewport.getWidth() / _viewport.getHeight();
// taken from glm
float range = nclip * tan(glFOV / 2 * (LOCAL_PI / 180));
float left = -range * aspect;
float right = range * aspect;
float bottom = -range;
float top = range;
Math::Matrix4 proj;
proj(0,0) = (2.0f * nclip) / (right - left);
proj(1,1) = (2.0f * nclip) / (top - bottom);
proj(2,0) = (right + left) / (right - left);
proj(2,1) = 0.0f; // (top + bottom) / (top - bottom);
proj(2,2) = -(fclip + nclip) / (fclip - nclip);
proj(2,3) = -1.0f;
proj(3,2) = -(2.0f * fclip * nclip) / (fclip - nclip);
proj(3,3) = 0.0f;
proj.transpose();
Math::Matrix4 model = Math::Quaternion::fromEuler(180.0f - heading, pitch, 0.0f).toMatrix();
model.transpose();
_mvpMatrix = proj * model;
_mvpMatrix.transpose();
}
void Transform::SetObjectTransform( const Math::Matrix4& transform )
{
Math::Scale scale;
Math::EulerAngles rotate;
Math::Vector3 translate;
transform.Decompose( scale, rotate, translate );
m_Scale = scale;
m_Rotate = rotate;
m_Translate = translate;
m_ObjectTransform = transform;
}
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;
}
void EMIHead::lookAt(bool entering, const Math::Vector3d &point, float rate, const Math::Matrix4 &matrix) {
if (!_cost->_emiSkel || !_cost->_emiSkel->_obj)
return;
if (_jointName.empty())
return;
Joint *joint = _cost->_emiSkel->_obj->getJointNamed(_jointName);
if (!joint)
return;
Math::Quaternion lookAtQuat; // Note: Identity if not looking at anything.
if (entering) {
Math::Matrix4 jointToWorld = _cost->getOwner()->getFinalMatrix() * joint->_finalMatrix;
Math::Vector3d jointWorldPos = jointToWorld.getPosition();
Math::Matrix4 worldToJoint = jointToWorld;
worldToJoint.invertAffineOrthonormal();
Math::Vector3d targetDir = (point + _offset) - jointWorldPos;
targetDir.normalize();
const Math::Vector3d worldUp(0, 1, 0);
Math::Vector3d frontDir = Math::Vector3d(worldToJoint(0, 1), worldToJoint(1, 1), worldToJoint(2, 1)); // Look straight ahead. (+Y)
Math::Vector3d modelFront(0, 0, 1);
Math::Vector3d modelUp(0, 1, 0);
joint->_absMatrix.inverseRotate(&modelFront);
joint->_absMatrix.inverseRotate(&modelUp);
// Generate a world-space look at matrix.
Math::Matrix4 lookAtTM;
lookAtTM.setToIdentity();
if (Math::Vector3d::dotProduct(targetDir, worldUp) >= 0.98f) // Avoid singularity if trying to look straight up.
lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, -frontDir); // Instead of orienting head towards scene up, orient head towards character "back",
else if (Math::Vector3d::dotProduct(targetDir, worldUp) <= -0.98f) // Avoid singularity if trying to look straight down.
lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, frontDir); // Instead of orienting head towards scene down, orient head towards character "front",
else
lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, worldUp);
// Convert from world-space to joint-space.
lookAtTM = worldToJoint * lookAtTM;
// Apply angle limits.
Math::Angle p, y, r;
lookAtTM.getXYZ(&y, &p, &r, Math::EO_ZXY);
y.clampDegrees(_yawRange);
p.clampDegrees(_minPitch, _maxPitch);
r.clampDegrees(30.0f);
lookAtTM.buildFromXYZ(y, p, r, Math::EO_ZXY);
lookAtQuat.fromMatrix(lookAtTM.getRotation());
}
if (_headRot != lookAtQuat) {
Math::Quaternion diff = _headRot.inverse() * lookAtQuat;
float angle = 2 * acos(diff.w());
if (diff.w() < 0.0f) {
angle = 2 * (float)M_PI - angle;
}
float turnAmount = g_grim->getPerSecond(rate * ((float)M_PI / 180.0f));
if (turnAmount < angle)
_headRot = _headRot.slerpQuat(lookAtQuat, turnAmount / angle);
else
_headRot = lookAtQuat;
}
if (_headRot != Math::Quaternion()) { // If not identity..
joint->_animMatrix = joint->_animMatrix * _headRot.toMatrix();
joint->_animQuat = joint->_animQuat * _headRot;
_cost->_emiSkel->_obj->commitAnim();
}
}
void Head::Joint::orientTowards(bool entering, const Math::Vector3d &point, float rate, const Math::Matrix4 &matrix,
float maxPitch, float maxYaw, float maxRoll, float constrain) {
float step = g_grim->getPerSecond(rate);
float yawStep = step;
float pitchStep = step / 3.0f;
float rollStep = step / 3.0f;
if (!_node)
return;
// Make sure we have up-to-date world transform matrices computed for the joint nodes of this character.
_node->_needsUpdate = true;
ModelNode *p = _node;
while (p->_parent) {
p = p->_parent;
p->_needsUpdate = true;
}
p->setMatrix(matrix);
p->update();
Math::Vector3d modelFront; // the modeling convention for the forward direction.
Math::Vector3d modelUp; // the modeling convention for the upward direction.
Math::Vector3d frontDir; // Character front facing direction vector in world space (global scene coordinate space)
// the character head coordinate frame is: +Y forward, +Z up, +X right.
frontDir = Math::Vector3d(_node->_matrix(0,1), _node->_matrix(1,1), _node->_matrix(2,1)); // Look straight ahead. (+Y)
modelFront = Math::Vector3d(0,1,0);
modelUp = Math::Vector3d(0,0,1);
// v is the world space direction vector this character should be looking towards.
Math::Vector3d targetDir = point - _node->_pivotMatrix.getPosition();
if (!entering)
targetDir = frontDir;
if (targetDir.isZero())
return;
targetDir.normalize();
// The vector v is in world space, so generate the world space lookat matrix for the desired head facing
// orientation.
Math::Matrix4 lookAtTM;
lookAtTM.setToIdentity();
const Math::Vector3d worldUp(0,0,1); // The Residual scene convention: +Z is world space up.
if (Math::Vector3d::dotProduct(targetDir, worldUp) >= 0.98f) // Avoid singularity if trying to look straight up.
lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, -frontDir); // Instead of orienting head towards scene up, orient head towards character "back",
// i.e. when you look straight up, your head up vector tilts/arches to point straight backwards.
else if (Math::Vector3d::dotProduct(targetDir, worldUp) <= -0.98f) // Avoid singularity if trying to look straight down.
lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, frontDir); // Instead of orienting head towards scene down, orient head towards character "front",
// i.e. when you look straight down, your head up vector tilts/arches to point straight forwards.
else
lookAtTM.buildFromTargetDir(modelFront, targetDir, modelUp, worldUp);
// The above specifies the world space orientation of this bone, but we need to output
// the orientation in parent space (as yaw/pitch/roll).
// Get the coordinate frame in which we need to produce the character head yaw/pitch/roll values.
Math::Matrix4 parentWorldTM;
if (_node->_parent)
parentWorldTM = _node->_parent->_matrix;
// While we could compute the desired lookat direction directly in the above coordinate frame,
// it is preferrable to compute the lookat direction with respect to the head orientation in
// the keyframe animation. This is because the LUA scripts specify the maximum head yaw, pitch and
// roll values with respect to those keyframe animations. If the lookat was simply computed
// directly in the space of the parent, we couldn't apply the head maxYaw/Pitch/Roll constraints
// properly. So, compute the coordinate frame of this bone in the keyframe animation.
Math::Matrix4 animFrame = _node->_localMatrix;
parentWorldTM = parentWorldTM * animFrame;
parentWorldTM.invertAffineOrthonormal();
// Convert lookAtTM orientation from world space to parent-with-keyframe-animation space.
lookAtTM = parentWorldTM * lookAtTM;
// Decompose to yaw-pitch-roll (+Z, +X, +Y).
// In this space, Yaw is +Z. Pitch is +X. Roll is +Y.
Math::Angle y, pt, r;
lookAtTM.getPitchYawRoll(&pt, &y, &r);
y = y * constrain;
pt = pt * constrain;
r = r * constrain;
// Constrain the maximum head movement, as desired by the game LUA scripts.
y.clampDegrees(maxYaw);
pt.clampDegrees(maxPitch);
r.clampDegrees(maxRoll);
// Also limit yaw, pitch and roll to make at most a movement as large as the given max step size during this frame.
// This will produce a slow head-turning animation instead of immediately snapping to the
// target lookat orientation.
if (y - _yaw > yawStep)
y = _yaw + yawStep;
if (_yaw - y > yawStep)
y = _yaw - yawStep;
if (pt - _pitch > pitchStep)
pt = _pitch + pitchStep;
if (_pitch - pt > pitchStep)
pt = _pitch - pitchStep;
if (r - _roll > rollStep)
r = _roll + rollStep;
if (_roll - r > rollStep)
r = _roll - rollStep;
// Remember how far we animated the head this frame, and we'll continue from here the next frame.
_pitch = pt;
_yaw = y;
_roll = r;
// Assemble ypr back to a matrix.
// This matrix is the head orientation with respect to parent-with-keyframe-animation space.
lookAtTM.buildFromPitchYawRoll(pt, y, r);
// What follows is a hack: Since translateObject(ModelNode *node, bool reset) in this file,
// and GfxOpenGL/GfxTinyGL::drawHierachyNode concatenate transforms incorrectly, by summing up
// euler angles, do a hack here where we do the proper transform here already, and *subtract off*
// the YPR scalars from the animYPR scalars to cancel out the values that those pieces of code
// will later accumulate. After those pieces of code have been fixed, the following lines can
// be deleted, and this function can simply output the contents of pt, y and r variables above.
lookAtTM = animFrame * lookAtTM;
lookAtTM.getPitchYawRoll(&pt, &y, &r);
_node->_animYaw = y - _node->_yaw;
_node->_animPitch = pt - _node->_pitch;
_node->_animRoll = r - _node->_roll;
}
void Head::lookAt(bool entering, const Math::Vector3d &point, float rate, const Math::Matrix4 &matrix) {
if (_joint1Node) {
float step = g_grim->getPerSecond(rate);
float yawStep = step;
float pitchStep = step / 3.f;
if (!entering) {
//animate yaw
if (_headYaw > yawStep) {
_headYaw -= yawStep;
} else if (_headYaw < -yawStep) {
_headYaw += yawStep;
} else {
_headYaw = 0;
}
//animate pitch
if (_headPitch > pitchStep) {
_headPitch -= pitchStep;
} else if (_headPitch < -pitchStep) {
_headPitch += pitchStep;
} else {
_headPitch = 0;
}
_joint1Node->_animYaw = _headYaw;
Math::Angle pi = _headPitch / 3.f;
_joint1Node->_animPitch += pi;
_joint2Node->_animPitch += pi;
_joint3Node->_animPitch += pi;
_joint1Node->_animRoll = (_joint1Node->_animYaw.getDegrees() / 20.f) *
_headPitch.getDegrees() / -5.f;
if (_joint1Node->_animRoll > _maxRoll)
_joint1Node->_animRoll = _maxRoll;
if (_joint1Node->_animRoll < -_maxRoll)
_joint1Node->_animRoll = -_maxRoll;
return;
}
ModelNode *p = _joint3Node;
while (p->_parent) {
p = p->_parent;
}
p->setMatrix(matrix);
p->update();
Math::Vector3d v = point - _joint3Node->_matrix.getPosition();
if (v.isZero()) {
return;
}
float magnitude = sqrt(v.x() * v.x() + v.y() * v.y());
float a = v.x() / magnitude;
float b = v.y() / magnitude;
float yaw;
yaw = acos(a) * (180.0f / LOCAL_PI);
if (b < 0.0f)
yaw = 360.0f - yaw;
Math::Angle bodyYaw = matrix.getYaw();
p = _joint1Node->_parent;
while (p) {
bodyYaw += p->_yaw + p->_animYaw;
p = p->_parent;
}
_joint1Node->_animYaw = (- 90 + yaw - bodyYaw);
if (_joint1Node->_animYaw < -180.) {
_joint1Node->_animYaw += 360;
}
if (_joint1Node->_animYaw > 180.) {
_joint1Node->_animYaw -= 360;
}
if (_joint1Node->_animYaw > _maxYaw)
_joint1Node->_animYaw = _maxYaw;
if (_joint1Node->_animYaw < -_maxYaw)
_joint1Node->_animYaw = -_maxYaw;
float sqLenght = v.x() * v.x() + v.y() * v.y();
float h;
if (sqLenght > 0) {
h = sqrt(sqLenght);
} else {
h = -sqrt(sqLenght);
}
magnitude = sqrt(v.z() * v.z() + h * h);
a = h / magnitude;
b = v.z() / magnitude;
Math::Angle pitch;
pitch = acos(a) * (180.0f / LOCAL_PI);
if (b < 0.0f)
pitch = 360.0f - pitch;
if (pitch > 180)
pitch -= 360;
if (pitch > _maxPitch)
pitch = _maxPitch;
if (pitch < -_maxPitch)
pitch = -_maxPitch;
if ((_joint1Node->_animYaw > 0 && pitch < 0) || (_joint1Node->_animYaw < 0 && pitch > 0)) {
pitch += _joint1Node->_animYaw / 10.f;
} else {
pitch -= _joint1Node->_animYaw / 10.f;
}
//animate pitch
if (pitch - _headPitch > pitchStep)
pitch = _headPitch + pitchStep;
if (_headPitch - pitch > pitchStep)
pitch = _headPitch - pitchStep;
Math::Angle pi = pitch / 3.f;
_joint1Node->_animPitch += pi;
_joint2Node->_animPitch += pi;
_joint3Node->_animPitch += pi;
//animate yaw
if (_joint1Node->_animYaw - _headYaw > yawStep)
_joint1Node->_animYaw = _headYaw + yawStep;
if (_headYaw - _joint1Node->_animYaw > yawStep)
_joint1Node->_animYaw = _headYaw - yawStep;
_joint1Node->_animRoll = (_joint1Node->_animYaw.getDegrees() / 20.f) *
pitch.getDegrees() / -5.f;
if (_joint1Node->_animRoll > _maxRoll)
_joint1Node->_animRoll = _maxRoll;
if (_joint1Node->_animRoll < -_maxRoll)
_joint1Node->_animRoll = -_maxRoll;
_headPitch = pitch;
_headYaw = _joint1Node->_animYaw;
}
}
void Frustum::setup(const Math::Matrix4 &matrix) {
// Based on "Fast Extraction of Viewing Frustum Planes from the
// World-View-Projection matrix" by Gil Gribb and Klaus Hartmann.
// http://www.cs.otago.ac.nz/postgrads/alexis/planeExtraction.pdf
_planes[0]._normal.x() = matrix.getValue(3, 0) + matrix.getValue(0, 0);
_planes[0]._normal.y() = matrix.getValue(3, 1) + matrix.getValue(0, 1);
_planes[0]._normal.z() = matrix.getValue(3, 2) + matrix.getValue(0, 2);
_planes[0]._d = matrix.getValue(3, 3) + matrix.getValue(0, 3);
_planes[1]._normal.x() = matrix.getValue(3, 0) - matrix.getValue(0, 0);
_planes[1]._normal.y() = matrix.getValue(3, 1) - matrix.getValue(0, 1);
_planes[1]._normal.z() = matrix.getValue(3, 2) - matrix.getValue(0, 2);
_planes[1]._d = matrix.getValue(3, 3) - matrix.getValue(0, 3);
_planes[2]._normal.x() = matrix.getValue(3, 0) - matrix.getValue(1, 0);
_planes[2]._normal.y() = matrix.getValue(3, 1) - matrix.getValue(1, 1);
_planes[2]._normal.z() = matrix.getValue(3, 2) - matrix.getValue(1, 2);
_planes[2]._d = matrix.getValue(3, 3) - matrix.getValue(1, 3);
_planes[3]._normal.x() = matrix.getValue(3, 0) + matrix.getValue(1, 0);
_planes[3]._normal.y() = matrix.getValue(3, 1) + matrix.getValue(1, 1);
_planes[3]._normal.z() = matrix.getValue(3, 2) + matrix.getValue(1, 2);
_planes[3]._d = matrix.getValue(3, 3) + matrix.getValue(1, 3);
_planes[4]._normal.x() = matrix.getValue(3, 0) + matrix.getValue(2, 0);
_planes[4]._normal.y() = matrix.getValue(3, 1) + matrix.getValue(2, 1);
_planes[4]._normal.z() = matrix.getValue(3, 2) + matrix.getValue(2, 2);
_planes[4]._d = matrix.getValue(3, 3) + matrix.getValue(2, 3);
_planes[5]._normal.x() = matrix.getValue(3, 0) - matrix.getValue(2, 0);
_planes[5]._normal.y() = matrix.getValue(3, 1) - matrix.getValue(2, 1);
_planes[5]._normal.z() = matrix.getValue(3, 2) - matrix.getValue(2, 2);
_planes[5]._d = matrix.getValue(3, 3) - matrix.getValue(2, 3);
for (int i = 0; i < 6; ++i) {
_planes[i].normalize();
}
}