Vector3F Viewport::Project(const Vector3F &source, const Matrix4 &projection, const Matrix4 &view, const Matrix4 &world) { Matrix4 mat = world * view * projection; // Matrix4::Multiply(Matrix4.Multiply(world, view), projection); Vector3F vector; mat.Transform(source, vector); // Vector3F::Transform(source, mat); float a = (((source.x * mat.a14) + (source.y * mat.a24)) + (source.z * mat.a34)) + mat.a44; if (!WithinEpsilon(a, 1.0f)) { vector = (Vector3F) (vector / a); } vector.x = (((vector.x + 1.0f) * 0.5f) * m_Width) + m_X; vector.y = (((-vector.y + 1.0f) * 0.5f) * m_Height) + m_Y; vector.z = (vector.z * (m_fFarClipPlane - m_fNearClipPlane)) + m_fNearClipPlane; return vector; }
Vector3F Viewport::Unproject(const Vector3F &source, const Matrix4 &projection, const Matrix4 &view, const Matrix4 &world) { Vector3F vector = source; Matrix4 mat = (world * view * projection).Invert(); //Matrix4.Invert(Matrix4.Multiply(Matrix4.Multiply(world, view), projection)); vector.x = (((source.x - m_X) / ((float) m_Width)) * 2.0f) - 1.0f; vector.y = -((((source.y - m_Y) / ((float) m_Height)) * 2.0f) - 1.0f); vector.z = (source.z - m_fNearClipPlane) / (m_fFarClipPlane - m_fNearClipPlane); mat.Transform(source, vector); // Vector3F.Transform(source, mat); float a = (((source.x * mat.a14) + (source.y * mat.a24)) + (source.z * mat.a34)) + mat.a44; if (!WithinEpsilon(a, 1.0f)) { vector = (Vector3F) (vector / a); } return vector; }
//***************************************************************************** // // * Set the quaternion related to two std::vector and one rotation axis //============================================================================ void SJCQuaterniond:: SetCoord(const SJCVector3d x_dir, const SJCVector3d up) //============================================================================ { SJCVector3d X, Y, Z, UP; X = x_dir.normal(); UP = up.normal(); double cos_x_up = up * x_dir; if(WithinEpsilon(cos_x_up, 1.f)) { double angle = -90.f * SJC_DEG_TO_RAD; set(angle, SJCConstants::SJC_vYAxis3d); return; } Y = UP % X; Y.normalize(); Z = X % Y; Z.normalize(); double m[3][3]; m[0][0] = X.x(); m[0][1] = X.y(); m[0][2] = X.z(); m[1][0] = Y.x(); m[1][1] = Y.y(); m[1][2] = Y.z(); m[2][0] = Z.x(); m[2][1] = Z.y(); m[2][2] = Z.z(); double tr, s, q[4]; int i, j, k; int nxt[3] = {1, 2, 0}; tr = m[0][0] + m[1][1] + m[2][2]; // check the diagonal if ( tr > 0.0 ) { s = sqrt (tr + 1.0); w_ = s / 2.0; s = 0.5 / s; x_ = (m[1][2] - m[2][1]) * s; y_ = (m[2][0] - m[0][2]) * s; z_ = (m[0][1] - m[1][0]) * s; } else { // diagonal is negative i = 0; if ( m[1][1] > m[0][0] ) i = 1; if ( m[2][2] > m[i][i] ) i = 2; j = nxt[i]; k = nxt[j]; s = sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0); q[i] = s * 0.5; if ( s > DBL_EPSILON ) s = 0.5 / s; q[3] = (m[j][k] - m[k][j]) * s; q[j] = (m[i][j] + m[j][i]) * s; q[k] = (m[i][k] + m[k][i]) * s; x_ = q[0]; y_ = q[1]; z_ = q[2]; w_ = q[3]; } normalize(); }