Matrix Matrix::operator*( const Matrix& mat ) const
{
Matrix matT;
matrixMultiply( &matT, this, &mat );
return matT;
}
void Renderer::setup_ortho(float left, float right, float bottom, float top, float near, float far)
{
float a = 2.0f / (right - left);
float b = 2.0f / (top - bottom);
float c = -2.0f / (far - near);
float tx = -(right + left) / 2.0f;
float ty = -(top + bottom) / 2.0f;
float tz = -(far + near) / (far - near);
mx_translate->Translate(tx, ty, tz);
mx_scale->Scale(a, b, c);
mx_rotate->Identity();
matrixMultiply(*mx_scale, *mx_translate, *ortho);
GLint projectionUniform = glGetUniformLocation(simple_shader, "Projection");
glUseProgram(simple_shader);
glUniformMatrix4fv(projectionUniform, 1, GL_FALSE, ortho->get_val());
GLint projectionUniformTex = glGetUniformLocation(_textureShader, "Projection");
glUseProgram(_textureShader);
glUniformMatrix4fv(projectionUniformTex, 1, GL_FALSE, ortho->get_val());
GLint projectionUniformBW = glGetUniformLocation(_bwShader, "Projection");
glUseProgram(_bwShader);
glUniformMatrix4fv(projectionUniformBW, 1, GL_FALSE, ortho->get_val());
GLint projectionUniformBlur = glGetUniformLocation(_blurShader, "Projection");
glUseProgram(_blurShader);
glUniformMatrix4fv(projectionUniformBlur, 1, GL_FALSE, ortho->get_val());
}
void FBMReference::computeModes( std::vector<double> &eigenvalues, std::vector<std::vector<Vec3> > &modes ) {
const unsigned int numModes = params.modes;
// Sort by ABSOLUTE VALUE of eigenvalues.
std::vector<EigenvalueColumn> sortedEvalues = sortEigenvalues( reducedSpaceEigenvalues );
TNT::Array2D<double> Q( reducedSpaceEigenvectors.dim1(), numModes, 0.0 );
for( int i = 0; i < numModes; i++ ) {
for( int j = 0; j < reducedSpaceEigenvectors.dim1(); j++ ) {
Q[j][i] = reducedSpaceEigenvectors[j][sortedEvalues[i].second];
}
}
TNT::Array2D<double> modeMatrix( projectionMatrix.dim1(), numModes, 0.0 );
matrixMultiply( projectionMatrix, Q, modeMatrix );
modes.resize( numModes, vector<Vec3>( particleCount ) );
for( unsigned int i = 0; i < numModes; i++ ) {
for( unsigned int j = 0; j < particleCount; j++ ) {
modes[i][j] = Vec3( modeMatrix[3 * j][i], modeMatrix[3 * j + 1][i], modeMatrix[3 * j + 2][i] );
}
}
eigenvalues.resize( numModes, 0.0 );
for( unsigned int i = 0; i < numModes; i++ ) {
eigenvalues[i] = sortedEvalues[i].first;
}
}
void loadUniforms() {
GLfloat ModelViewProjection[4*4], NormalMatrix[3*3];
matrixMultiply(ModelViewProjection, Projection, ModelView);
matrixNormal(ModelView, NormalMatrix);
glUniformMatrix4fv(ModelViewProjectionUniform, 1, GL_FALSE,
ModelViewProjection);
glUniformMatrix4fv(ModelViewMatrixUniform, 1, GL_FALSE,
ModelView);
glUniformMatrix3fv(NormalMatrixUniform, 1, GL_FALSE, NormalMatrix);
//
// Load lights.
//
glUniform3fv(ambientLightUniform, 1, ambientLight);
glUniform3fv(light0ColorUniform, 1, light0Color);
glUniform3fv(light0PositionUniform, 1, light0Position);
//
// Load material properties.
//
glUniform3fv(materialAmbientUniform, 1, materialAmbient);
glUniform3fv(materialDiffuseUniform, 1, materialDiffuse);
glUniform3fv(materialSpecularUniform, 1, materialSpecular);
glUniform1f(materialShininessUniform, materialShininess);
}
void matrixRotate(GLfloat M[4*4],
GLfloat angle_degrees, GLfloat x, GLfloat y, GLfloat z) {
const GLfloat p = 1/sqrtf(x*x + y*y + z*z);
x *= p; y *= p; z *= p;
const float angle = angle_degrees * (M_PI/180);
const float c = cosf(angle);
const float s = sinf(angle);
const float c_ = 1 - c;
const float zc_ = z*c_;
const float yc_ = y*c_;
const float xzc_ = x*zc_;
const float xyc_ = x*y*c_;
const float yzc_ = y*zc_;
const float xs = x*s;
const float ys = y*s;
const float zs = z*s;
GLfloat S[4*4];
matrixSet3x3(S,
x*x*c_ + c, xyc_ - zs, xzc_ + ys,
xyc_ + zs, y*yc_ + c, yzc_ - xs,
xzc_ - ys, yzc_ + xs, z*zc_ + c);
GLfloat T[4*4];
matrixMultiply(T,M,S);
matrixCopy(M, T);
}
/*! Given an array of desired joint values, this computed an infinitesimal motion
of each link as motion *from the current values* towards the desired values is
started. Used mainly to see if any contacts prevent this motion.
*/
void
KinematicChain::infinitesimalMotion(const double *jointVals, std::vector<transf> &newLinkTranVec) const
{
//start with the link jacobian in local link coordinates
//but keep just the actuated part
Matrix J(actuatedJacobian(linkJacobian(false)));
//joint values matrix
Matrix theta(numJoints, 1);
//a very small motion
//either 0.1 radians or 0.1 mm, should be small enough
double inf = 0.1;
//a very small threshold
double eps = 1.0e-6;
for(int j=0; j<numJoints; j++) {
int sign;
if ( jointVals[firstJointNum + j] + eps < jointVec[j]->getVal() ) {
sign = -1;
} else if ( jointVals[firstJointNum + j] > jointVec[j]->getVal() + eps ) {
sign = 1;
} else {
sign = 0;
}
theta.elem(j,0) = sign * inf;
}
//compute infinitesimal motion
Matrix dm(6*numLinks, 1);
matrixMultiply(J, theta, dm);
//and convert it to transforms
for (int l=0; l<numLinks; l++) {
transf tr = rotXYZ( dm.elem(6*l+3, 0), dm.elem(6*l+4, 0), dm.elem(6*l+5, 0) ) *
translate_transf( vec3( dm.elem(6*l+0, 0), dm.elem(6*l+1, 0), dm.elem(6*l+2, 0) ) );
newLinkTranVec[l] = tr;
}
}
void getTriplesByPerimeter(triple * t, const uint64_t limit, triple *** const list, uint32_t * const count, uint32_t * const size) {
static const int64_t transformationMatrix[3][3][3] =
{{{ 1,-2, 2}
,{ 2,-1, 2}
,{ 2,-2, 3}}
,{{ 1, 2, 2}
,{ 2, 1, 2}
,{ 2, 2, 3}}
,{{-1, 2, 2}
,{-2, 1, 2}
,{-2, 2, 3}}};
uint32_t i;
uint64_t perimeter_ = perimeter(t);
if(perimeter_ > limit) {
free(t);
return;
}
addToList(t,list,count,size);
for(i=2; i*perimeter_ <= limit; ++i)
addToList(scalarMultiply(i,t),list,count,size);
for(i=0; i<3; ++i)
getTriplesByPerimeter(matrixMultiply((int64_t *)transformationMatrix[i],t)
,limit,list,count,size);
}
/*! The matrix, when multiplied with a wrench applied at this contact will give the
resultant wrench applied on the other body in contact (thus computed relative
to that object's center of mass), expressed in world coordinates.
The matrix looks like this:
| R 0 |
|CR R |
Where R is the 3x3 rotation matrix between the coordinate systems and C also
contains the cross product matrix that depends on the translation between them.
*/
Matrix
Contact::localToWorldWrenchMatrix() const
{
Matrix Ro(Matrix::ZEROES<Matrix>(6,6));
transf contactTran = getContactFrame() * getBody1()->getTran();
mat3 R; contactTran.rotation().ToRotationMatrix(R);
Matrix Rot( Matrix::ROTATION(R) );
//the force transform is simple, just the matrix that changes coord. systems
Ro.copySubMatrix(0, 0, Rot);
Ro.copySubMatrix(3, 3, Rot);
//for torque we also multiply by a cross product matrix
vec3 worldLocation = contactTran.translation();
vec3 cog = getBody2()->getTran().translation();
mat3 C; C.setCrossProductMatrix(worldLocation - cog);
Matrix CR(3,3);
matrixMultiply(Matrix::ROTATION(C.transpose()), Rot, CR);
//also scale by object max radius so we get same units as force
//and optimization does not favor torque over force
double scale = 1.0;
if (getBody2()->isA("GraspableBody")) {
scale = scale / static_cast<GraspableBody*>(getBody2())->getMaxRadius();
}
CR.multiply(scale);
Ro.copySubMatrix(3, 0, CR);
return Ro;
}
void loadUniforms() {
GLfloat ModelViewProjection[4*4];
matrixMultiply(ModelViewProjection, Projection, ModelView);
glUniformMatrix4fv(ModelViewProjectionUniform, 1, GL_FALSE,
ModelViewProjection);
glUniform3fv(ColorUniform, 1, Color);
}
int
McGripGrasp::computeQuasistaticForces(double tendonForce)
{
//routing matrices
Matrix *B, *a;
static_cast<McGrip *>(hand)->getRoutingMatrices(&B, &a);
//parameter matrix
Matrix p(8, 1);
//tendon insertion points
p.elem(0, 0) = 5;
p.elem(1, 0) = 5;
p.elem(2, 0) = 1.65;
//--------------
p.elem(3, 0) = 5;
p.elem(4, 0) = 5;
p.elem(5, 0) = 1.65;
//link length and joint radius
p.elem(6, 0) = static_cast<McGrip *>(hand)->getJointRadius();
p.elem(7, 0) = static_cast<McGrip *>(hand)->getLinkLength();
//compute joint torques
Matrix tau(6, 1);
matrixMultiply(*B, p, tau);
matrixAdd(tau, *a, tau);
delete a;
delete B;
//multiply by tendon force
tau.multiply(tendonForce);
//call super
return Grasp::computeQuasistaticForces(tau);
}
/*! One possible formulation of the core GFO problem. Finds the contacts
forces that result in 0 object wrench and are as far as possible
from the edges of the friction cones. Assumes that at least one set
of contact forces that satisfy this criterion exist; see
contactForceExistence for that problem. See inner comments for exact
mathematical formulation. Not for standalone use; is called by the GFO
functions in the Grasp class.
*/
int
contactForceOptimization(Matrix &F, Matrix &N, Matrix &Q, Matrix &beta, double *objVal, Matrix * objectWrench = NULL)
{
// exact problem formulation
// unknowns: beta (contact forces)
// minimize sum * F * beta (crude measure of friction resistance abilities)
// subject to:
// Q * beta = 0 (0 resultant object wrench)
// sum_normal * beta = 1 (we are applying some contact forces)
// F * beta <= 0 (each individual forces inside friction cones)
// beta >= 0 (all forces must be positive)
// overall equality constraint:
// | Q | |beta| |0|
// | N | = |1|
//right hand of equality
Matrix right_hand = Matrix::ZEROES<Matrix>(Q.rows()+1, 1);
if(objectWrench)
{
assert(objectWrench->rows() == Q.rows());
for(int i = 0; i < Q.rows(); ++i)
right_hand.elem(i,0) = objectWrench->elem(i,0);
}
//a total of 10N of normal force
right_hand.elem(Q.rows(),0) = 1.0e7;
//left hand of equality
Matrix LeftHand(Q.rows() + 1, Q.cols());
LeftHand.copySubMatrix(0, 0, Q);
LeftHand.copySubMatrix(Q.rows(), 0, N);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta.rows(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(beta.rows()));
//objective: sum of F
Matrix FSum(1,F.rows());
FSum.setAllElements(1.0);
Matrix FObj(1,F.cols());
matrixMultiply(FSum, F, FObj);
/*
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"Left Hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
F.print(fp);
fprintf(fp,"Objective:\n");
Q.print(fp);
fclose(fp);
*/
int result = LPSolver(FObj, LeftHand, right_hand, F, Matrix::ZEROES<Matrix>(F.rows(), 1),
lowerBounds, upperBounds,
beta, objVal);
return result;
}
void readMPU6050(void)
{
uint8_t axis;
uint8_t I2C2_Buffer_Rx[14];
int16_t straightAccelData[3];
int16_t rotatedAccelData[3];
int16_t straightGyroData[3];
int16_t rotatedGyroData[3];
i2cRead(MPU6050_ADDRESS, MPU6050_ACCEL_XOUT_H, 14, I2C2_Buffer_Rx);
rawAccel[YAXIS].bytes[1] = I2C2_Buffer_Rx[ 0];
rawAccel[YAXIS].bytes[0] = I2C2_Buffer_Rx[ 1];
rawAccel[XAXIS].bytes[1] = I2C2_Buffer_Rx[ 2];
rawAccel[XAXIS].bytes[0] = I2C2_Buffer_Rx[ 3];
rawAccel[ZAXIS].bytes[1] = I2C2_Buffer_Rx[ 4];
rawAccel[ZAXIS].bytes[0] = I2C2_Buffer_Rx[ 5];
rawMPU6050Temperature.bytes[1] = I2C2_Buffer_Rx[ 6];
rawMPU6050Temperature.bytes[0] = I2C2_Buffer_Rx[ 7];
rawGyro[PITCH].bytes[1] = I2C2_Buffer_Rx[ 8];
rawGyro[PITCH].bytes[0] = I2C2_Buffer_Rx[ 9];
rawGyro[ROLL ].bytes[1] = I2C2_Buffer_Rx[10];
rawGyro[ROLL ].bytes[0] = I2C2_Buffer_Rx[11];
rawGyro[YAW ].bytes[1] = I2C2_Buffer_Rx[12];
rawGyro[YAW ].bytes[0] = I2C2_Buffer_Rx[13];
for (axis = 0; axis < 3; axis++)
{
straightAccelData[axis] = rawAccel[axis].value;
straightGyroData[axis] = rawGyro[axis].value;
}
matrixMultiply(3, 3, 1, rotatedAccelData, orientationMatrix, straightAccelData);
matrixMultiply(3, 3, 1, rotatedGyroData, orientationMatrix, straightGyroData);
for (axis = 0; axis < 3; axis++)
{
rawAccel[axis].value = rotatedAccelData[axis];
rawGyro[axis].value = rotatedGyroData[axis];
}
}
/*! Computes the grasp map G. D is the matrix that relates friction edge
amplitudes to contact force and R is the matrix that transorms contact
forces to world coordinate system. We then need to sum all of them up
by multiplication with a summation matrix to get the grasp map
G = S R D
G relates friction edge amplitudes from all contacts to resultant
object wrench.
*/
Matrix
Grasp::graspMapMatrix(const Matrix &R, const Matrix &D)
{
int numContacts = R.rows() / 6;
assert(6 * numContacts == R.rows());
//summation matrix that adds full 6D object wrenches
Matrix S(6, 6*numContacts);
for(int i=0; i<numContacts; i++) {
S.copySubMatrix(0, 6*i, Matrix::EYE(6,6));
}
Matrix SR(S.rows(), R.cols());
matrixMultiply(S, R, SR);
Matrix G(S.rows(), D.cols());
matrixMultiply(SR, D, G);
return G;
}
void
fxFlyinUpdateWindowTransform (CompWindow *w,
CompTransform *wTransform)
{
ANIMSIM_WINDOW(w);
matrixMultiply (wTransform, wTransform, &aw->com->transform);
}
void matrixTranslate(float* matrix, float x, float y, float z)
{
float temporaryMatrix[16];
matrixIdentityFunction(temporaryMatrix);
temporaryMatrix[12] = x;
temporaryMatrix[13] = y;
temporaryMatrix[14] = z;
matrixMultiply(matrix,temporaryMatrix,matrix);
}
void matrixScale(float* matrix, float x, float y, float z)
{
float tempMatrix[16];
matrixIdentityFunction(tempMatrix);
tempMatrix[0] = x;
tempMatrix[5] = y;
tempMatrix[10] = z;
matrixMultiply(matrix, tempMatrix, matrix);
}
Matrix& Matrix::operator*=( const Matrix& mat )
{
Matrix matT;
matrixMultiply( &matT, this, &mat );
*this = matT;
return( *this );
}
void loadUniforms() {
GLfloat ModelViewProjection[4*4];
glGetFloatv(GL_MODELVIEW_MATRIX, ModelView);
glGetFloatv(GL_PROJECTION_MATRIX, Projection);
matrixMultiply(ModelViewProjection, Projection, ModelView);
glUniformMatrix4fv(ModelViewProjectionUniform, 1, GL_FALSE,
ModelViewProjection);
glUniform3fv(ColorUniform, 1, Color);
}
void
EigenGraspInterface::toDOFSpace(const double *amp, double *dof, const double *origin) const
{
Matrix a(amp, eSize, 1, true);
Matrix x(dSize, 1);
matrixMultiply(*mPInv, a, x);
for(int d=0; d<dSize; d++) {
dof[d] = x.elem(d,0) * mNorm->getAxisValue(d) + origin[d];
}
}
void matrixTranslate(GLfloat M[4*4], GLfloat dx, GLfloat dy, GLfloat dz) {
GLfloat S[4*4];
matrixSet3x4(S,
1, 0, 0, dx,
0, 1, 0, dy,
0, 0, 1, dz);
GLfloat T[4*4];
matrixMultiply(T,M,S);
matrixCopy(M, T);
}
void matrixRotateZ(float *matrix, float angle)
{
float tempMatrix[16];
matrixIdentityFunction(tempMatrix);
tempMatrix[0] = cos(matrixDegreesToRadians(angle));
tempMatrix[4] = -sin(matrixDegreesToRadians(angle));
tempMatrix[1] = sin(matrixDegreesToRadians(angle));
tempMatrix[5] = cos(matrixDegreesToRadians(angle));
matrixMultiply(matrix, tempMatrix, matrix);
}
void matrixScale(GLfloat M[4*4], GLfloat sx, GLfloat sy, GLfloat sz) {
GLfloat S[4*4];
matrixSet3x3(S,
sx, 0, 0,
0, sy, 0,
0, 0, sz);
GLfloat T[4*4];
matrixMultiply(T,M,S);
matrixCopy(M, T);
}
// Update the dCM matrix to caculate the orientation
void AnglesMatrixUpdate(float gyroscopeVector[3], int gyroscopeTimestamp)
{
double period;
if(timestampPreviousCall==0)
period = 0;
else
period = (gyroscopeTimestamp - timestampPreviousCall); //time in seconds between the two last measures we got
double temporaryVector[3]= {0,0,0};
double updateMatrix[3][3]; //temporary matrix we use to update the dCMMatrix
double temporaryMatrix[3][3];
double gyroscopeVectorInvertedSystem[3]; //We call here Inverted System the Device coordinate system with Dx = Dy and Dy = Dx( the axis are exchanged)
/****** We have to Exchange the x and y value of our vector to correspond with the system used by the algorithm *******/
gyroscopeVectorInvertedSystem[0] = gyroscopeVector[1];
gyroscopeVectorInvertedSystem[1] = gyroscopeVector[0];
gyroscopeVectorInvertedSystem[2] = gyroscopeVector[2];
/******Calculate the GyroscopeVectorCorrected which is the gyroscope vector measure plus some vectors we get with the drift correction ( corresponding to the PID of the algorithm ********/
vectorAddition(temporaryVector, gyroscopeVectorInvertedSystem, integratorVector); //adding proportional term
vectorAddition(gyroscopeVectorCorrected, temporaryVector, proportionalVector); //adding Integrator term
/******We calculate the changement that the new measurement will imply*******/
updateMatrix[0][0]=0;
updateMatrix[0][1]=-period*gyroscopeVectorCorrected[2];
updateMatrix[0][2]=period*gyroscopeVectorCorrected[1];
updateMatrix[1][0]=period*gyroscopeVectorCorrected[2];
updateMatrix[1][1]=0;
updateMatrix[1][2]=-period*gyroscopeVectorCorrected[0];
updateMatrix[2][0]=-period*gyroscopeVectorCorrected[1];
updateMatrix[2][1]=period*gyroscopeVectorCorrected[0];
updateMatrix[2][2]=0;
/******We update the DCM Matrix *******/
matrixMultiply(temporaryMatrix,dCMMatrix, updateMatrix);
for(int x=0; x<3; x++) //Matrix Addition (update)
{
for(int y=0; y<3; y++)
{
dCMMatrix[x][y]+=temporaryMatrix[x][y];
}
}
timestampPreviousCall = gyroscopeTimestamp;
}
//Check if the ray intersects with a triangle.
bool intersectTriangle(Ray &ray, Triangle &tr, double &t, double &u, double &v, Point &normal){
double a[3][3];
double b[9];
double c[3];
double result[3];
double det;
a[0][0] = -ray.d.x;
a[1][0] = -ray.d.y;
a[2][0] = -ray.d.z;
a[0][1] = tr.v[1].position[0] - tr.v[0].position[0];
a[1][1] = tr.v[1].position[1] - tr.v[0].position[1];
a[2][1] = tr.v[1].position[2] - tr.v[0].position[2];
a[0][2] = tr.v[2].position[0] - tr.v[0].position[0];
a[1][2] = tr.v[2].position[1] - tr.v[0].position[1];
a[2][2] = tr.v[2].position[2] - tr.v[0].position[2];
det= a[0][0] * (a[1][1] * a[2][2] - a[2][1] * a[1][2]) - a[0][1] * (a[1][0] * a[2][2] - a[1][2] * a[2][0]) + a[0][2] * (a[1][0] * a[2][1] - a[1][1] * a[2][0]);
b[0]= (a[1][1] * a[2][2] - a[2][1] * a[1][2])/det;
b[1]= -(a[1][0] * a[2][2] - a[1][2] * a[2][0])/det;
b[2]= (a[1][0] * a[2][1] - a[2][0] * a[1][1])/det;
b[3]= -(a[0][1] * a[2][2] - a[0][2] * a[2][1])/det;
b[4]= (a[0][0] * a[2][2] - a[0][2] * a[2][0])/det;
b[5]= -(a[0][0] * a[2][1] - a[2][0] * a[0][1])/det;
b[6]= (a[0][1] * a[1][2] - a[0][2] * a[1][1])/det;
b[7]= -(a[0][0] * a[1][2] - a[1][0] * a[0][2])/det;
b[8]= (a[0][0] * a[1][1] - a[1][0] * a[0][1])/det; // matrix inverse
c[0] = ray.p.x - tr.v[0].position[0];
c[1] = ray.p.y - tr.v[0].position[1];
c[2] = ray.p.z - tr.v[0].position[2];
matrixMultiply(c, b, result, 1, 3, 3);
if ( result[0] < 1E-8){
return false;
}
double temp = 1 - result[1] - result[2];
if(result[1]>= 1E-8 && result[1] <= 1 && result[2] >= 1E-8 && result[2] <= 1 && temp <= 1 && temp >= 1E-8){
t = result[0];
u = result[1];
v = result[2];
double s = 1 - u - v;
normal.x = s * tr.v[0].normal[0] + u * tr.v[1].normal[0] + v * tr.v[2].normal[0];
normal.y = s * tr.v[0].normal[1] + u * tr.v[1].normal[1] + v * tr.v[2].normal[1];
normal.z = s * tr.v[0].normal[2] + u * tr.v[1].normal[2] + v * tr.v[2].normal[2]; // calculate normal
return true;
}
return false;
}
void
EigenGraspInterface::toEigenSpace(double *amp, const double *dof, const double *origin) const
{
Matrix x(dSize, 1);
for (int d=0; d < dSize; d++) {
x.elem(d,0) = (dof[d] - origin[d]) / mNorm->getAxisValue(d);
}
Matrix a(eSize, 1);
matrixMultiply(*mP, x, a);
for (int e=0; e<eSize; e++) {
amp[e] = a.elem(e,0);
}
}
////////////////////////////////////////////////////////////////////////////////
// Program main
////////////////////////////////////////////////////////////////////////////////
int main(int argc, char **argv)
{
printf("[Matrix Multiply CUBLAS] - Starting...\n");
int devID = 0, sizeMult = 5;
sMatrixSize matrix_size;
initializeCUDA(argc, argv, devID, sizeMult, matrix_size);
int matrix_result = matrixMultiply(argc, argv, devID, matrix_size);
return matrix_result;
}
void rotateMatrix(double axis[], double mRotate[][4], double mTransform[][4], double radian)
{
int i,j;
double u[3],v[3],t[3];
double tmp[4][4];
vecUnitization(axis,axis);
t[0] = axis[0];
t[1] = axis[1]+1;
t[2] = axis[2];
vecCross(t,axis,u);
vecUnitization(u,u);
vecCross(axis,u,v);
vecUnitization(v,v);
matrixInitial(tmp);
matrixInitial(mRotate);
tmp[0][0] = cos(radian);
tmp[0][1] = -sin(radian);
tmp[1][0] = sin(radian);
tmp[1][1] = cos(radian);
for(i = 0; i < 3; i++){
mRotate[0][i] = u[i];
mRotate[1][i] = v[i];
mRotate[2][i] = axis[i];
}
matrixMultiply(tmp,mRotate,0);
matrixInitial(tmp);
for(i = 0; i < 3; i++){
tmp[i][0] = u[i];
tmp[i][1] = v[i];
tmp[i][2] = axis[i];
}
matrixMultiply(tmp,mRotate,0);
matrixMultiply(mRotate,mTransform,0);
}
void matrixOrtho(GLfloat M[4*4],
GLfloat left, GLfloat right,
GLfloat bottom, GLfloat top,
GLfloat hither, GLfloat yon) {
GLfloat S[4*4];
matrixSet3x4(S,
2/(right - left), 0, 0, -(right + left)/(right - left),
0, 2/(top - bottom), 0, -(top + bottom)/(top - bottom),
0, 0, -2/(yon-hither), -(yon + hither)/(yon - hither));
GLfloat T[4*4];
matrixMultiply(T,M,S);
matrixCopy(M, T);
}
// Define the view frustum for culling
C3DTFrustum viewFrustum(const C3DTMatrix projection, const C3DTMatrix model)
{
C3DTMatrix clip;
C3DTFrustum r;
clip = matrixMultiply(projection, model);
// Right plane
r.planes[0].cartesian.x = clip.flts[3] - clip.flts[0];
r.planes[0].cartesian.y = clip.flts[7] - clip.flts[4];
r.planes[0].cartesian.z = clip.flts[11] - clip.flts[8];
r.planes[0].cartesian.w = clip.flts[15] - clip.flts[12];
r.planes[0] = vectorNormalize(r.planes[0]);
// Left plane
r.planes[1].cartesian.x = clip.flts[3] + clip.flts[0];
r.planes[1].cartesian.y = clip.flts[7] + clip.flts[4];
r.planes[1].cartesian.z = clip.flts[11] + clip.flts[8];
r.planes[1].cartesian.w = clip.flts[15] + clip.flts[12];
r.planes[1] = vectorNormalize(r.planes[1]);
// Bottom plane
r.planes[2].cartesian.x = clip.flts[3] + clip.flts[1];
r.planes[2].cartesian.y = clip.flts[7] + clip.flts[5];
r.planes[2].cartesian.z = clip.flts[11] + clip.flts[9];
r.planes[2].cartesian.w = clip.flts[15] + clip.flts[13];
r.planes[2] = vectorNormalize(r.planes[2]);
// Top plane
r.planes[3].cartesian.x = clip.flts[3] - clip.flts[1];
r.planes[3].cartesian.y = clip.flts[7] - clip.flts[5];
r.planes[3].cartesian.z = clip.flts[11] - clip.flts[9];
r.planes[3].cartesian.w = clip.flts[15] - clip.flts[13];
r.planes[3] = vectorNormalize(r.planes[3]);
// Far plane
r.planes[4].cartesian.x = clip.flts[3] - clip.flts[2];
r.planes[4].cartesian.y = clip.flts[7] - clip.flts[6];
r.planes[4].cartesian.z = clip.flts[11] - clip.flts[10];
r.planes[4].cartesian.w = clip.flts[15] - clip.flts[14];
r.planes[4] = vectorNormalize(r.planes[4]);
// Near plane
r.planes[5].cartesian.x = clip.flts[3] + clip.flts[2];
r.planes[5].cartesian.y = clip.flts[7] + clip.flts[6];
r.planes[5].cartesian.z = clip.flts[11] + clip.flts[10];
r.planes[5].cartesian.w = clip.flts[15] + clip.flts[14];
r.planes[5] = vectorNormalize(r.planes[5]);
return r;
}
void
EigenGraspInterface::computeProjectionMatrices()
{
if (mP) delete mP;
if (mPInv) delete mPInv;
//first build the E matrix that just contains the (potentially non-orthonormal) bases
Matrix E(eSize, dSize);
for (int e=0; e<eSize; e++) {
for (int d=0; d<dSize; d++) {
E.elem(e,d) = mGrasps[e]->getAxisValue(d);
}
}
//the trivial case (assumes ortho-normal E)
//mP = new Matrix(E);
//mPInv = new Matrix(E.transposed());
//general case
//remember: P(PInv(a)) = a (always)
// PInv(P(x)) != x (usually)
// P = (E*ET)'*E
// P' = ET
Matrix ET(E.transposed());
Matrix EET(eSize, eSize);
matrixMultiply(E, ET, EET);
Matrix EETInv(eSize, eSize);
int result = matrixInverse(EET, EETInv);
if (result) {
DBGA("Projection matrix is rank deficient!");
mP = new Matrix(Matrix::ZEROES<Matrix>(eSize, dSize));
mPInv = new Matrix(Matrix::ZEROES<Matrix>(dSize, eSize));
return;
}
mP = new Matrix(eSize, dSize);
matrixMultiply(EETInv, E, *mP);
mPInv = new Matrix(ET);
}