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(); } }