Matrix3x3 CPhysicsActor::GetGlobalOrientation()
{
Matrix3x3 orientation;
NxMat33 tmp = m_Actor->getGlobalOrientation();
tmp.getColumnMajor( orientation.GetMatrix() );
return orientation;
}
//-----------------------------------------------------------------------
void Matrix3x3::EigenSolveSymmetric (double afEigenvalue[3],
Vector3 akEigenvector[3]) const
{
Matrix3x3 kMatrix = *this;
double afSubDiag[3];
kMatrix.Tridiagonal(afEigenvalue,afSubDiag);
kMatrix.QLAlgorithm(afEigenvalue,afSubDiag);
for (size_t i = 0; i < 3; i++)
{
akEigenvector[i][0] = kMatrix[0][i];
akEigenvector[i][1] = kMatrix[1][i];
akEigenvector[i][2] = kMatrix[2][i];
}
// make eigenvectors form a right--handed system
Vector3 kCross = akEigenvector[1].CrossProduct(akEigenvector[2]);
double fDet = akEigenvector[0].DotProduct(kCross);
if ( fDet < 0.0 )
{
akEigenvector[2][0] = - akEigenvector[2][0];
akEigenvector[2][1] = - akEigenvector[2][1];
akEigenvector[2][2] = - akEigenvector[2][2];
}
}
void Quaternion::transformToMatrix4x4(double *matrix4x4) const {
if (matrix4x4 != NULL) {
Matrix3x3 result = transformToMatrix3x3();
matrix4x4[0] = result.getXX();
matrix4x4[1] = result.getXY();
matrix4x4[2] = result.getXZ();
matrix4x4[3] = 0;
// Second row
matrix4x4[4] = result.getYX();
matrix4x4[5] = result.getYY();
matrix4x4[6] = result.getYZ();
matrix4x4[7] = 0;
// Third row
matrix4x4[8] = result.getZX();
matrix4x4[9] = result.getZY();
matrix4x4[10] = result.getZZ();
matrix4x4[11] = 0.;
// Fourth row
matrix4x4[12] = 0;
matrix4x4[13] = 0;
matrix4x4[14] = 0;
matrix4x4[15] = 1;
}
}
bool CMatrix4x4<T>::GetInverseMatrix(CMatrix4x4 &matInverse) const
{
T nDeterminate = Determinate();
int i, j, sign;
if(fabs(nDeterminate) < 0.0005)
{
return false;
}
for(i = 0; i < 4; i++)
{
for(j = 0; j < 4; j++)
{
sign = 1 - ((i+j)%2) * 2;
Matrix3x3<T> matSub = GetSubMatrix3x3(i,j);
T det = matSub.Determinate();
matInverse.m_Entries[i+j*4] = (det * sign)/nDeterminate;
}//end for
}//end for
return true;
}//end GetInverseMatrix
// Given an edge, the constructor for EdgeRecord finds the
// optimal point associated with the edge's current quadric,
// and assigns this edge a cost based on how much quadric
// error is observed at this optimal point.
EdgeRecord::EdgeRecord( EdgeIter& _edge )
: edge( _edge )
{
// TODO Compute the combined quadric from the edge endpoints.
Matrix4x4 q = _edge->halfedge()->vertex()->quadric +
_edge->halfedge()->twin()->vertex()->quadric;
// TODO Build the 3x3 linear system whose solution minimizes
// the quadric error associated with these two endpoints.
Matrix3x3 quadratic;
quadratic(0,0) = q(0,0); quadratic(0,1) = q(0,1); quadratic(0,2) = q(0,2);
quadratic(1,0) = q(1,0); quadratic(1,1) = q(1,1); quadratic(1,2) = q(1,2);
quadratic(2,0) = q(2,0); quadratic(2,1) = q(2,1); quadratic(2,2) = q(2,2);
Vector3D linear(q(3,0), q(3,1), q(3,2));
// TODO Use this system to solve for the optimal position, and
// TODO store it in EdgeRecord::optimalPoint.
optimalPoint = - quadratic.inv() * linear;
// TODO Also store the cost associated with collapsing this edge
// TODO in EdgeRecord::Cost.
Vector4D optH(optimalPoint);
optH.w = 1.0;
score = dot(optH, q * optH);
}
void PPC::SetPMat(){
Matrix3x3 cam;
cam.setColumn(0, a);
cam.setColumn(1, b);
cam.setColumn(2, c);
pMat = cam.inverse();
}
Matrix3x3 &Matrix3x3::Clone( ) const
{
Matrix3x3 *pClone = new Matrix3x3( );
pClone->Copy( *this );
return *pClone;
}
TEST(Matrix3x3, Sum)
{
const Matrix3x3<int> matrix1(1, 2, 3, 4, 5, 6, 7, 8, 9);
const Matrix3x3<double> matrix2(10.08, 11.10, 24.5, 24.0, 30.70, 50.50, 70.90, 80.05, 100.70);
EXPECT_EQ(matrix1.sum(), 45);
EXPECT_EQ(matrix2.sum(), 402.53);
}
void Node::setRotation( const Matrix3x3& rotation )
{
assert( rotation.getColumn(0).finite() );
assert( rotation.getColumn(1).finite() );
assert( rotation.getColumn(2).finite() );
assert( ( rotation.getColumn(0).cross(rotation.getColumn(1)) ).dot( rotation.getColumn(2) ) > 0.f ); // Left handed coordinate system?
m_localTransform.setRotation( rotation );
m_flags |= NODE_WORLDTMDIRTY;
}
TEST(Matrix3x3, Sort)
{
const Matrix3x3<int> matrix1(1, 10, 2, 20, 15, 30, 5, 100, 50);
const Matrix3x3<double> matrix2(1.0, 10.5, -0.5, 100.0, 0.0, 7.0, 15.0, 8.0, 30.0);
const Matrix3x3<int> result1(1, 2, 5, 10, 15, 20, 30, 50, 100);
const Matrix3x3<double> result2(-0.5, 0.0, 1.0, 7.0, 8.0, 10.5, 15.0, 30.0, 100.0);
EXPECT_TRUE(matrix1.sort() == result1);
EXPECT_TRUE(matrix2.sort() == result2);
}
TEST(M3x3, opMulti)
{
Matrix3x3 testMatrix0;
testMatrix0.FillMatrix(1,2,2,3,4,5,6,7,9);
Matrix3x3 testMatrix1;
testMatrix1.FillMatrix(4, 1, 2, 3, 6, 3, 2, 1, 7);
Matrix3x3 result;
result.FillMatrix(14, 15, 22, 34, 32, 53, 63, 57, 96);
EXPECT_EQ(result, (testMatrix0*testMatrix1));
}
TEST(Matrix3x3, Compare)
{
const Matrix3x3<int> matrix1(100, 200, 300, 400, 500, 600, 700, 800, 900);
Matrix3x3<int> matrix2 = matrix1;
EXPECT_TRUE(matrix1 == matrix2);
matrix2.set(0, 0, matrix1.get(0, 0) + 1);
EXPECT_FALSE(matrix1 == matrix2);
}
Point3 Point3::operator*(const Matrix3x3 &m) const {
const double x = getX() * m.getXX() + getY() * m.getXY() + getZ() * m.getXZ();
const double y = getX() * m.getYX() + getY() * m.getYY() + getZ() * m.getYZ();
const double z = getX() * m.getZX() + getY() * m.getZY() + getZ() * m.getZZ();
Point3 cc(x, y, z);
return cc;
}
Matrix3x3 Matrix3x3::operator*(const Matrix3x3 &right) const
{
Matrix3x3 ret;
for (unsigned int iRow = 0; iRow < 3; iRow++)
{
for (unsigned int iCol = 0; iCol < 3; iCol++)
{
ret.SetCell(iRow, iCol, this->GetRow(iRow) * right.GetColumn(iCol));
}
}
return ret;
}
Matrix3x3 Matrix3x3::operator*(const double &right) const
{
Matrix3x3 ret = *this;
for (unsigned int iRow = 0; iRow < 3; iRow++)
{
for (unsigned int iCol = 0; iCol < 3; iCol++)
{
ret.SetCell(iRow,iCol, right * m_dValues[iRow][iCol]);
}
}
return ret;
}
//check if points of the triangle is counterclockwise
bool cclock( float x0, float y0,
float x1, float y1,
float x2, float y2 ) {
Matrix3x3 m;
m(0,0) = x0; m(0,1) = x1; m(0,2) = x2;
m(1,0) = y0; m(1,1) = y1; m(1,2) = y2;
m(2,0) = 1 ; m(2,1) = 1 ; m(2,2) = 1 ;
return m.det() >= 0.0;
/*
return lineSide(x2 - x0, y2 - y0, x0, y0, x1, y1);
*/
}
// this should be moved to the math classes
// NEED NEW VEC CLASS
Vec3 rotateVectorAxisAngle( const Vec3 &vec, const Vec3 &axis, const float angle )
{
Matrix3x3 mat;
mat.SetIdentity();
mat.SetFromAxisAngle( axis, angle );
Vec3 transformed;
//need to check the matrix type here carefully
transformed.x = mat.m[0]*vec.x + mat.m[1]*vec.y + mat.m[2]*vec.z;
transformed.y = mat.m[3]*vec.x + mat.m[4]*vec.y + mat.m[5]*vec.z;
transformed.z = mat.m[6]*vec.x + mat.m[7]*vec.y + mat.m[8]*vec.z;
return transformed;
}
Matrix3x3& Matrix3x3::operator*=(const Matrix3x3 &right)
{
Matrix3x3 temp;
for (unsigned int iRow = 0; iRow < 3; iRow++)
{
for (unsigned int iCol = 0; iCol < 3; iCol++)
{
temp.SetCell(iRow, iCol, this->GetRow(iRow) * right.GetColumn(iCol));
}
}
*this = temp;
return *this;
}
Matrix3x3 Matrix3x3::MulMatrix(Matrix3x3 matrix)
{
Matrix3x3 ret;
Matrix3x1 col;
for(int i = 0; i < 3; i++)
{
col = this->MulMatrix3X1(matrix.GetColumn(i));
ret.SetColumn(col, i);
}
return ret;
}
TEST(Matrix3x3, Transform)
{
Matrix3x3 position;
position.transform(Vector2F(2.0f, 3.0f),
Vector2F(1.0f, 1.0f),
0.0,
Vector2F());
Matrix3x3 exPosition(1.0f, 0.0f, 2.0f,
0.0f, 1.0f, 3.0f,
0.0f, 0.0f, 1.0f);
EXPECT_EQ(exPosition, position);
Matrix3x3 pivot;
pivot.transform(Vector2F(),
Vector2F(1.0f, 1.0f),
0.0,
Vector2F(2.0f, 3.0f));
EXPECT_EQ(exPosition, pivot);
Matrix3x3 rotation;
rotation.transform(Vector2F(),
Vector2F(1.0f, 1.0f),
180.0,
Vector2F());
Matrix3x3 exRotation(-1.0f, 0.0f, 0.0f,
0.0f, -1.0f, 0.0f,
0.0f, 0.0f, 1.0f);
EXPECT_EQ(exRotation, rotation);
Matrix3x3 scale;
scale.transform(Vector2F(),
Vector2F(2.0f, 3.0f),
0.0,
Vector2F());
Matrix3x3 exScale(2.0f, 0.0f, 0.0f,
0.0f, 3.0f, 0.0f,
0.0f, 0.0f, 1.0f);
EXPECT_EQ(exScale, scale);
// NOTE: Matrix equality checks if the values are close. Because this
// uses trig the rotation values do not line up to 0.0.
Matrix3x3 all;
all.transform(Vector2F(5.0f, 6.0f),
Vector2F(2.0f, 3.0f),
180.0,
Vector2F(10.0f, 20.0f));
Matrix3x3 exAll(-2.0f, 0.0f, -15.0f,
0.0f, -3.0f, -54.0f,
0.0f, 0.0f, 1.0f);
EXPECT_EQ(exAll, all);
}
void PS3GCMCGEffect::setUniform(const Matrix3x3& uniformData, const char* uniformName) const {
{
CGparameter parameter = cellGcmCgGetNamedParameter(vertexProgram_, uniformName);
if (parameter) {
cellGcmSetVertexProgramParameter(parameter, uniformData.valuePtr());
}
}
{
CGparameter parameter = cellGcmCgGetNamedParameter(fragmentProgram_, uniformName);
if (parameter) {
cellGcmSetFragmentProgramParameter(fragmentProgram_, parameter, uniformData.valuePtr(), fragmentProgramOffset_);
}
}
}
a2de::Matrix3x3 Matrix3x3::operator*(const Matrix3x3& rhs) {
a2de::Vector3D r1(this->GetRowOne());
a2de::Vector3D r2(this->GetRowTwo());
a2de::Vector3D r3(this->GetRowThree());
a2de::Vector3D c1(rhs.GetColumnOne());
a2de::Vector3D c2(rhs.GetColumnTwo());
a2de::Vector3D c3(rhs.GetColumnThree());
return Matrix3x3(a2de::Vector3D::DotProduct(r1, c1), a2de::Vector3D::DotProduct(r1, c2), a2de::Vector3D::DotProduct(r1, c3),
a2de::Vector3D::DotProduct(r2, c1), a2de::Vector3D::DotProduct(r2, c2), a2de::Vector3D::DotProduct(r2, c3),
a2de::Vector3D::DotProduct(r3, c1), a2de::Vector3D::DotProduct(r3, c2), a2de::Vector3D::DotProduct(r3, c3));
}
/**
* @brief
* Called on scene node position change or update request
*/
void SNMRotationTarget::OnPositionUpdate()
{
// Get the position of the target scene node
Vector3 vTargetPos;
if (GetTargetPosition(vTargetPos)) {
// Get a quaternion representation of the rotation offset
Quaternion qRotOffset;
EulerAngles::ToQuaternion(static_cast<float>(Offset.Get().x*Math::DegToRad), static_cast<float>(Offset.Get().y*Math::DegToRad), static_cast<float>(Offset.Get().z*Math::DegToRad), qRotOffset);
// Set rotation
Matrix3x3 mRot;
mRot.LookAt(vTargetPos, GetSceneNode().GetTransform().GetPosition(), UpVector.Get());
GetSceneNode().GetTransform().SetRotation(Quaternion(mRot).GetUnitInverted()*qRotOffset);
}
}
void process_input(Matrix3x3 &M){
char command[1024];
bool done;
float theta, s, cx, cy;
float sx, sy, shx, shy, px, py;
int dx, dy;
/* build identity matrix */
M.identity();
for(done = false; !done;) {
/* prompt and accept input, converting text to lower case */
cout << "> ";
cin >> command;
lowercase(command);
/* parse the input command, and read parameters as needed */
if(strcmp(command, "d") == 0) {
done = true;
imageData = Manipulation::warper(imageData, M);
}
else if(strcmp(command, "n") == 0) {// twirl
if(cin >> s >> cx >> cy)
imageData = Manipulation::twirl(imageData, s, cx, cy);
else
cout << "invalid twirl parameter\n";
done = true;
}
else if(strlen(command) != 1) {
/**
* @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space
* coordinates
*/
void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
mUserInertiaTensorLocalInverse = inertiaTensorLocal.getInverse();
mIsInertiaTensorSetByUser = true;
if (mType != BodyType::DYNAMIC) return;
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mUserInertiaTensorLocalInverse;
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mID) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
}
a2de::Matrix3x3 Matrix3x3::Inverse(const Matrix3x3& mat) {
//Minors, Cofactors, Adjugates method.
//See http://www.mathsisfun.com/algebra/matrix-inverse-minors-cofactors-adjugate.html
//[00 01 02] [0 1 2]
//[10 11 12] [3 4 5]
//[20 21 22] [6 7 8]
//Calculate minors
double m00 = Matrix2x2::CalculateDeterminant(Matrix2x2(mat._indicies[4], mat._indicies[5], mat._indicies[7], mat._indicies[8]));
double m01 = Matrix2x2::CalculateDeterminant(Matrix2x2(mat._indicies[3], mat._indicies[5], mat._indicies[6], mat._indicies[7]));
double m02 = Matrix2x2::CalculateDeterminant(Matrix2x2(mat._indicies[3], mat._indicies[4], mat._indicies[6], mat._indicies[7]));
double m10 = Matrix2x2::CalculateDeterminant(Matrix2x2(mat._indicies[1], mat._indicies[2], mat._indicies[7], mat._indicies[8]));
double m11 = Matrix2x2::CalculateDeterminant(Matrix2x2(mat._indicies[0], mat._indicies[2], mat._indicies[6], mat._indicies[7]));
double m12 = Matrix2x2::CalculateDeterminant(Matrix2x2(mat._indicies[0], mat._indicies[1], mat._indicies[6], mat._indicies[7]));
double m20 = Matrix2x2::CalculateDeterminant(Matrix2x2(mat._indicies[1], mat._indicies[2], mat._indicies[4], mat._indicies[5]));
double m21 = Matrix2x2::CalculateDeterminant(Matrix2x2(mat._indicies[0], mat._indicies[2], mat._indicies[3], mat._indicies[5]));
double m22 = Matrix2x2::CalculateDeterminant(Matrix2x2(mat._indicies[0], mat._indicies[1], mat._indicies[3], mat._indicies[4]));
Matrix3x3 cofactors(m00, -m01, m02,
-m10, m11, -m12,
m20, -m21, m22);
Matrix3x3 adjugate(Matrix3x3::Transpose(cofactors));
double det_mat = mat.CalculateDeterminant();
double inv_det = 1.0 / det_mat;
return inv_det * adjugate;
}
void transformPoint(T& x, T& y, T& z, const Matrix3x3<T>& Rmat, const Point3<T>& Tvec ) {
T nx, ny, nz;
const T* m = Rmat.data();
nx = m[0] * x + m[1] * y + m[2] * z + Tvec.x;
ny = m[3] * x + m[4] * y + m[5] * z + Tvec.y;
nz = m[6] * x + m[7] * y + m[8] * z + Tvec.z;
x = nx, y = ny, z = nz;
}
void rotatePoint(T& x, T& y, T& z, const Matrix3x3<T>& Rmat ) {
T nx, ny, nz;
const T* m = Rmat.data();
nx = m[0] * x + m[1] * y + m[2] * z;
ny = m[3] * x + m[4] * y + m[5] * z;
nz = m[6] * x + m[7] * y + m[8] * z;
x = nx, y = ny, z = nz;
}
Matrix3x3 Quaternion::transformToMatrix3x3() const {
Matrix3x3 result;
result.setXX(1.0 - 2*(getY()*getY() + getZ()*getZ()));
result.setXY(2*(getX()*getY() + getZ()*getW()));
result.setXZ(2*(getX()*getZ() - getY()*getW()));
result.setYX(2*(getX()*getY() - getZ()*getW()));
result.setYY(1.0 - 2*(getX()*getX() + getZ()*getZ()));
result.setYZ(2*(getZ()*getY() + getX()*getW()));
result.setZX(2*(getX()*getZ() + getY()*getW()));
result.setZY(2*(getY()*getZ() - getX()*getW()));
result.setZZ(1.0 - 2*(getX()*getX() + getY()*getY()));
return result;
}
virtual bool OverlapObjects_GiantBoxVsKP::CommonSetup()
{
TestBase::CommonSetup();
LoadMeshesFromFile_(*this, "kp.bin");
const Point Min(-9560.175781f, -1093.885132f, -9461.288086f);
const Point Max(9538.423828f, 4906.125488f, 9637.304688f);
const Point Center = (Max + Min)*0.5f;
const Point Extents = (Max - Min)*0.5f;
Matrix3x3 Rot;
Rot.Identity();
// Rot.RotYX(0.1f, 0.2f);
RegisterBoxOverlap(OBB(Center, Point(2500.0f, 2500.0f, 2500.0f), Rot));
mCreateDefaultEnvironment = false;
return true;
}
