Matrix4 Matrix4::CreateLookAtRightHanded(Vector3& position, Vector3& lookAt, Vector3& upVector)
{
Matrix4 perspective = Matrix4::Zero();
Vector3 forward = (lookAt - position).Normalize();
Vector3 right = upVector.CrossProduct(forward).Normalize();
Vector3 up = forward.CrossProduct(right).Normalize();
perspective[0][0] = -right.GetX();
perspective[1][0] = -right.GetY();
perspective[2][0] = -right.GetZ();
perspective[3][0] = right.ScalarProduct(position);
perspective[0][1] = up.GetX();
perspective[1][1] = up.GetY();
perspective[2][1] = up.GetZ();
perspective[3][1] = -up.ScalarProduct(position);
perspective[0][2] = -forward.GetX();
perspective[1][2] = -forward.GetY();
perspective[2][2] = -forward.GetZ();
perspective[3][2] = forward.ScalarProduct(position);
perspective[0][3] = 0.0f;
perspective[1][3] = 0.0f;
perspective[2][3] = 0.0f;
perspective[3][3] = 1.0f;
return perspective;
}
void Matrix3::SetBlockInertiaTensor(const Vector3& halfSizes, real mass)
{
Vector3 squares = halfSizes.ComponentProduct(halfSizes);
SetInertiaTensorCoeffs(0.3f*mass*(squares.GetY() + squares.GetZ()),
0.3f*mass*(squares.GetX() + squares.GetZ()),
0.3f*mass*(squares.GetX() + squares.GetY()));
}
const float Dot ( const Vector3& aLhs, const Vector3& aRhs )
{
return
aLhs.GetX() * aRhs.GetX() +
aLhs.GetY() * aRhs.GetY() +
aLhs.GetZ() * aRhs.GetZ();
}
const Vector3 Cross ( const Vector3& aLhs, const Vector3& aRhs )
{
return Vector3 (
( aLhs.GetY() * aRhs.GetZ() ) - ( aLhs.GetZ() * aRhs.GetY() ),
( aLhs.GetZ() * aRhs.GetX() ) - ( aLhs.GetX() * aRhs.GetZ() ),
( aLhs.GetX() * aRhs.GetY() ) - ( aLhs.GetY() * aRhs.GetX() ) );
}
Vector3 Vector3::CrossProduct(const Vector3& other) const
{
Vector3 result;
result.SetX( (m_Y * other.GetZ()) - (m_Z * other.GetY()) );
result.SetY( (m_Z * other.GetX()) - (m_X * other.GetZ()) );
result.SetZ( (m_X * other.GetY()) - (m_Y * other.GetX()) );
return result;
}
//normalise the vectors
Vector3 Vector3::Normalise(Vector3 v) const
{
Vector3 result;
float mag;
mag = sqrt(v.GetX() * v.GetX() + v.GetY() * v.GetY() + v.GetZ() * v.GetZ());
result = v / mag;
return result;
}
void Matrix3::Multiply(const Vector3& other, Vector3* result) const
{
//@REF: Transform the matrix by the vector
result->SetX((other.GetX()*m_Elems[0]) +
(other.GetY()*m_Elems[1]) +
(other.GetZ()*m_Elems[2]));
result->SetY((other.GetX()*m_Elems[3]) +
(other.GetY()*m_Elems[4]) +
(other.GetZ()*m_Elems[5]));
result->SetZ((other.GetX()*m_Elems[6]) +
(other.GetY()*m_Elems[7]) +
(other.GetZ()*m_Elems[8]));
}
CMatrix4 CMatrix4::LookAt(Vector3 position, Vector3 target, Vector3 up)
{
Vector3 zaxis = position - target;
zaxis.Normalize();
Vector3 xaxis = Vector3::CrossProduct(up, zaxis);
xaxis.Normalize();
Vector3 yaxis = Vector3::CrossProduct(zaxis, xaxis);
return CMatrix4(
xaxis.GetX(), xaxis.GetY(), xaxis.GetZ(), -Vector3::DotProduct(xaxis, position),
yaxis.GetX(), yaxis.GetY(), yaxis.GetZ(), -Vector3::DotProduct(yaxis, position),
zaxis.GetX(), zaxis.GetY(), zaxis.GetZ(), -Vector3::DotProduct(zaxis, position),
0.0f, 0.0f, 0.0f, 1.0f);
}
void Matrix3x3::SetColumns( const Vector3 &p_Column1,
const Vector3 &p_Column2, const Vector3 &p_Column3 )
{
m_M[ 0 ] = p_Column1.GetX( );
m_M[ 1 ] = p_Column1.GetY( );
m_M[ 2 ] = p_Column1.GetZ( );
m_M[ 3 ] = p_Column2.GetX( );
m_M[ 4 ] = p_Column2.GetY( );
m_M[ 5 ] = p_Column2.GetZ( );
m_M[ 6 ] = p_Column3.GetX( );
m_M[ 7 ] = p_Column3.GetY( );
m_M[ 8 ] = p_Column3.GetZ( );
}
Vector3 Matrix3::Multiply(const Vector3& other) const
{
Vector3 result;
//@REF: Transform the matrix by the vector
result.SetX((other.GetX()*m_Elems[0]) +
(other.GetY()*m_Elems[1]) +
(other.GetZ()*m_Elems[2]));
result.SetY((other.GetX()*m_Elems[3]) +
(other.GetY()*m_Elems[4]) +
(other.GetZ()*m_Elems[5]));
result.SetZ((other.GetX()*m_Elems[6]) +
(other.GetY()*m_Elems[7]) +
(other.GetZ()*m_Elems[8]));
return result;
}
Matrix4 Matrix4::Rotate(float angle, Vector3& factors)
{
Matrix4 rotation;
Vector3 f = factors.Normalize();
float x = f.GetX();
float y = f.GetY();
float z = f.GetZ();
float s = sin(angle);
float c = cos(angle);
rotation[0][0] = x*x * (1-c) + c;
rotation[0][1] = x*y * (1-c) + z*s;
rotation[0][2] = x*z * (1-c) - y*s;
rotation[0][3] = 0.0f;
rotation[1][0] = y*x * (1-c) - z*s;
rotation[1][1] = y*y * (1-c) + c;
rotation[1][2] = y*z * (1-c) + x*s;
rotation[1][3] = 0.0f;
rotation[2][0] = x*z * (1-c) + y*s;
rotation[2][1] = y*z * (1-c) - x*s;
rotation[2][2] = z*z * (1-c) + c;
rotation[2][3] = 0.0f;
rotation[3][0] = 0.0f;
rotation[3][1] = 0.0f;
rotation[3][2] = 0.0f;
rotation[3][3] = 1.0f;
return rotation;
}
void AABB::computeAABB(const Vector3& position, const Vector3& dimension)
{
float aabb[6];
aabb[0] = position.GetX() + dimension.GetX() / 2;
aabb[1] = position.GetY() - dimension.GetY() / 2;
aabb[2] = position.GetZ() + dimension.GetZ() / 2;
aabb[3] = position.GetX() - dimension.GetX() / 2;
aabb[4] = position.GetY() + dimension.GetY() / 2;
aabb[5] = position.GetZ() - dimension.GetZ() / 2;
SIMDVector3 min(aabb[0], aabb[1], aabb[2]);
SIMDVector3 max(aabb[3], aabb[4], aabb[5]);
m_Max = max;
m_Min = min;
}
bool CCollisionBody::CheckSATCollision( CCollisionInfo & info, Vector3< float > & curPos, CCollisionBody * c, Vector3< float > & cPos ) {
unsigned long int curAxisCount = m_Axis.size();
unsigned long int totalAxisCount = curAxisCount + c->GetAxisCount();
for( int j = 0; j < totalAxisCount; j++ ) {
Vector3< float > curAxis = ( j >= curAxisCount )? c->GetAxis( j - curAxisCount ) : m_Axis[j];
float minA, maxA;
float minB, maxB;
FindSATMinMax( m_ColOffset, curPos, curAxis, &minA, &maxA );
FindSATMinMax( c->GetOffsets(), cPos, curAxis, &minB, &maxB );
double d;
int t = 0;
if( minA < minB )
d = minB - maxA;
else {
t = 1;
d = minA - maxB;
}
if( d > 0 ) {
return false;
} else {
if( minA < minB )
curAxis = curAxis * -1.0;
//Ensure second colliding object has this normal
if( c->HasNormal( curAxis.GetX(), curAxis.GetY(), curAxis.GetZ() ) ) {
if( fabs( d ) < info.minTransDist ) {
info.minTransDist = fabs( d );
info.minTransAxis = curAxis;
}
}
}
}
info.collision = true;
return true;
}
const Vector3 Vector3::operator-(const Vector3 &other) const
{
Vector3 result;
result.SetX(_x - other.GetX());
result.SetY(_y - other.GetY());
result.SetZ(_z - other.GetZ());
return result;
}
const Vector3 Vector3::operator+(const Vector3 &other) const
{
Vector3 result;
result.SetX(_x + other.GetX());
result.SetY(_y + other.GetY());
result.SetZ(_z + other.GetZ());
return result;
}
Vector3 Vector3::Add(const Vector3& other) const
{
Vector3 result;
result.SetX(m_X + other.GetX());
result.SetY(m_Y + other.GetY());
result.SetZ(m_Z + other.GetZ());
return result;
}
Vector3 Vector3::Subtract(const Vector3& other) const
{
Vector3 result;
result.SetX(m_X - other.GetX());
result.SetY(m_Y - other.GetY());
result.SetZ(m_Z - other.GetZ());
return result;
}
Vector3 Vector3::ComponentProduct(const Vector3& other) const
{
Vector3 result;
result.SetX(m_X * other.GetX());
result.SetY(m_Y * other.GetY());
result.SetZ(m_Z * other.GetZ());
return result;
}
Matrix4 Matrix4::Translate(Vector3& v)
{
Matrix4 translation = Identity();
translation[3][0] = v.GetX();
translation[3][1] = v.GetY();
translation[3][2] = v.GetZ();
return translation;
}
real Vector3::DotProduct(const Vector3& other) const
{
real result = 0.0f;
result += (m_X * other.GetX());
result += (m_Y * other.GetY());
result += (m_Z * other.GetZ());
return result;
}
Vector3 Matrix3x3::operator*( const Vector3 &p_Vector )
{
KIL_FLOAT32 X, Y, Z;
X = m_M[ 0 ] * p_Vector.GetX( ) +
m_M[ 3 ] * p_Vector.GetY( ) +
m_M[ 6 ] * p_Vector.GetZ( );
Y = m_M[ 1 ] * p_Vector.GetX( ) +
m_M[ 4 ] * p_Vector.GetY( ) +
m_M[ 7 ] * p_Vector.GetZ( );
Z = m_M[ 2 ] * p_Vector.GetX( ) +
m_M[ 5 ] * p_Vector.GetY( ) +
m_M[ 8 ] * p_Vector.GetZ( );
return Vector3( X, Y, Z );
}
void RendererOpenGL::TransformCamera(const Vector3& position, const Vector3& orientation) const
{
// Rotate the object On Z, X & Y - work in progress
glRotatef(-orientation.GetX(), 0, 1, 0);
glRotatef(-orientation.GetY(), 1, 0, 0);
// Move the object
glTranslatef(-position.GetX(), -position.GetY(), -position.GetZ());
}
const Vector3 Normalize ( const Vector3& aVector )
{
float magnitude = sqrtf ( Dot ( aVector, aVector ) );
assert ( magnitude != 0.0f );
return Vector3 (
aVector.GetX() / magnitude,
aVector.GetY() / magnitude,
aVector.GetZ() / magnitude );
}
bool Shader::SetShaderVec3Parameter(const char* name, Vector3 vec3)
{
int locID = glGetUniformLocation(m_programID, name);
if (locID >= 0)
{
glUniform3f(locID, vec3.GetX(), vec3.GetY(), vec3.GetZ());
}
return true;
}
Matrix4 Matrix4::Scale(Vector3& factors)
{
Matrix4 scaled = Matrix4::Identity();
scaled[0][0] = factors.GetX();
scaled[1][1] = factors.GetY();
scaled[2][2] = factors.GetZ();
return scaled;
}
TEST(Vector, Blend)
{
Vector3 v1(1.0f, 1.0f, 1.0f);
Vector3 v2(4.0f, 4.0f, 4.0f);
Vector3 v3(8.0f, 8.0f, 8.0f);
Vector3 v4(16.0f, 16.0f, 16.0f);
Vector3 vBlendResult = Blend(v1, v2, v3, v4, 0.25f, 0.25f, 0.25f);
EXPECT_NEAR(7.25f, vBlendResult.GetZ(), 0.01f);
}
//cross product
Vector3 Vector3::crossProduct(const Vector3& v) const
{
Vector3 result;
result.SetX(_y * v.GetZ() - v.GetY() * _z);
result.SetY(_x * v.GetZ() - v.GetX() * _z);
result.SetZ(_x * v.GetY() - v.GetX() * _y);
return result;
}
AABB::AABB(const Matrix4 & transform, const Vector3 & dimension, const Vector3 & origin)
{
float aabb[6];
aabb[0] = origin.GetX() + dimension.GetX() / 2;
aabb[1] = origin.GetY() - dimension.GetY() / 2;
aabb[2] = origin.GetZ() + dimension.GetZ() / 2;
aabb[3] = origin.GetX() - dimension.GetX() / 2;
aabb[4] = origin.GetY() + dimension.GetY() / 2;
aabb[5] = origin.GetZ() - dimension.GetZ() / 2;
Vector3 min(aabb[0], aabb[1], aabb[2]);
Vector3 max(aabb[3], aabb[4], aabb[5]);
Vector3 translate(transform.getTranslateX(), transform.getTranslateY(), transform.getTranslateZ());
min += translate;
max += translate;
m_Max = max;
m_Min = min;
Body::setType(1);
}
void Quaternion::AddScaledVector(const Vector3& vec, real scale)
{
Quaternion q(0,
vec.GetX() * scale,
vec.GetY() * scale,
vec.GetZ() * scale);
q *= *this;
m_R += q.GetR() * ((real)0.5);
m_I += q.GetI() * ((real)0.5);
m_J += q.GetJ() * ((real)0.5);
m_K += q.GetK() * ((real)0.5);
}
void Point3D::Translate(Vector3 &iDir, double idist)
{
double d1 = _pMatrix->GetAt(0)+iDir.GetX();
double d2 = _pMatrix->GetAt(1)+iDir.GetY();
double d3 = _pMatrix->GetAt(2)+iDir.GetZ();
if(_pMatrix)
{
_pMatrix->SetAt(0, &d1);
_pMatrix->SetAt(1, &d2);
_pMatrix->SetAt(2, &d3);
}
}