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