bool invertMatrix(MatrixType& A, MatrixType& AI)
{
typedef typename MatrixType::ElemType Real;
if (A.getNRow() != A.getNCol() || A.getNRow() != AI.getNRow() || A.getNCol() != AI.getNCol()) {
return false;
}
int size = A.getNRow();
int *index=NULL, iScratch[10];
Real *column=NULL, dScratch[10];
// Check on allocation of working vectors
//
if ( size <= 10 ) {
index = iScratch;
column = dScratch;
} else {
index = new int[size];
column = new Real[size];
}
bool retVal = invertMatrix(A, AI, size, index, column);
if ( size > 10 ) {
delete [] index;
delete [] column;
}
return retVal;
}
void setLightMatrix(double *result, double *shadowProjection, double *shadowModelView, double* camModelView) {
/*Matrix44f shadowTransMatrix = mShadowCam.getProjectionMatrix();
shadowTransMatrix *= mShadowCam.getModelViewMatrix();
shadowTransMatrix *= camera.getInverseModelViewMatrix();
return shadowTransMatrix;*/
// Adding the BIAS means we can use shadow2DProj which is good for linear filtering on the FBO for free with NVidia cards
double bias[16] = {
0.5, 0.0, 0.0, 0.0,
0.0, 0.5, 0.0, 0.0,
0.0, 0.0, 0.5, 0.0,
0.5, 0.5, 0.5, 1.0};
double trans[16];
multMatrix(trans, bias, shadowProjection);
double trans2[16];
multMatrix(trans2,trans,shadowModelView);
//setEqual(trans, shadowModelView);
double inv[16];
invertMatrix(inv, camModelView);
multMatrix(result, trans2, inv);
}
HydroProp* Sphere::getHydroProp(RealType viscosity, RealType temperature) {
RealType Xitt = 6.0 * NumericConstant::PI * viscosity * radius_;
RealType Xirr = 8.0 * NumericConstant::PI * viscosity * radius_ * radius_ * radius_;
Mat6x6d Xi, XiCopy, D;
Xi(0, 0) = Xitt;
Xi(1, 1) = Xitt;
Xi(2, 2) = Xitt;
Xi(3, 3) = Xirr;
Xi(4, 4) = Xirr;
Xi(5, 5) = Xirr;
Xi *= PhysicalConstants::viscoConvert;
XiCopy = Xi;
invertMatrix(XiCopy, D);
RealType kt = PhysicalConstants::kb * temperature; // in kcal mol^-1
D *= kt; // now in angstroms^2 fs^-1 (at least for Trans-trans)
HydroProp* hprop = new HydroProp(V3Zero, Xi, D);
return hprop;
}
void OE_ProjectionMgr::ApplyToObj(void)
{
if (gObjects.empty())
return;
GLdouble matrix[16], matrix_i[16];
mDeformer->GetTransform(matrix);
invertMatrix(matrix_i, matrix);
mPreviewObj = gObjects[gLevelOfDetail];
for (set<int>::iterator cmdNum = gSelection.begin(); cmdNum != gSelection.end(); ++cmdNum)
{
XObjCmd& cmd = mPreviewObj.cmds[*cmdNum];
for (vector<vec_tex>::iterator st = cmd.st.begin(); st != cmd.st.end(); ++st)
{
GLdouble xyz[4], xyz_t[4];
xyz[0] = st->v[0];
xyz[1] = st->v[1];
xyz[2] = st->v[2];
xyz[3] = 1.0;
double s, t;
multMatrixVec(xyz_t, matrix_i, xyz);
mProjector[mCurProjector]->ProjectVertex(xyz_t[0], xyz_t[1], xyz_t[2], s, t);
s = mTexture.s1 + s * (mTexture.s2 - mTexture.s1);
t = mTexture.t1 + t * (mTexture.t2 - mTexture.t1);
st->st[0] = s;
st->st[1] = t;
}
}
}
void
Nonlinear3D::computeIncrementalDeformationGradient(std::vector<ColumnMajorMatrix> & Fhat)
{
// A = grad(u(k+1) - u(k))
// Fbar = 1 + grad(u(k))
// Fhat = 1 + A*(Fbar^-1)
ColumnMajorMatrix A;
ColumnMajorMatrix Fbar;
ColumnMajorMatrix Fbar_inverse;
ColumnMajorMatrix Fhat_average;
Real volume(0);
_Fbar.resize(_solid_model.qrule()->n_points());
for (unsigned qp = 0; qp < _solid_model.qrule()->n_points(); ++qp)
{
fillMatrix(qp, _grad_disp_x, _grad_disp_y, _grad_disp_z, A);
fillMatrix(qp, _grad_disp_x_old, _grad_disp_y_old, _grad_disp_z_old, Fbar);
A -= Fbar;
Fbar.addDiag(1);
_Fbar[qp] = Fbar;
// Get Fbar^(-1)
// Computing the inverse is generally a bad idea.
// It's better to compute LU factors. For now at least, we'll take
// a direct route.
invertMatrix(Fbar, Fbar_inverse);
Fhat[qp] = A * Fbar_inverse;
Fhat[qp].addDiag(1);
if (_volumetric_locking_correction)
{
// Now include the contribution for the integration of Fhat over the element
Fhat_average += Fhat[qp] * _solid_model.JxW(qp);
volume += _solid_model.JxW(qp); // Accumulate original configuration volume
}
}
if (_volumetric_locking_correction)
{
Fhat_average /= volume;
const Real det_Fhat_average(detMatrix(Fhat_average));
// Finalize volumetric locking correction
for (unsigned qp = 0; qp < _solid_model.qrule()->n_points(); ++qp)
{
const Real det_Fhat(detMatrix(Fhat[qp]));
const Real factor(std::cbrt(det_Fhat_average / det_Fhat));
Fhat[qp] *= factor;
}
}
// Moose::out << "Fhat(0,0)" << Fhat[0](0,0) << std::endl;
}
const Matrix&
SectionForceDeformation2d::getInitialFlexibility ()
{
int order = this->getOrder();
if (fDefault == 0) {
fDefault = new Matrix(order,order);
if (fDefault == 0) {
opserr << "SectionForceDeformation2d::getInitialFlexibility -- failed to allocate flexibility matrix\n";
exit(-1);
}
}
const Matrix &k = this->getInitialTangent();
switch(order) {
case 1:
if (k(0,0) != 0.0)
(*fDefault)(0,0) = 1.0/k(0,0);
break;
case 2:
invert2by2Matrix(k,*fDefault);
break;
case 3:
invert3by3Matrix(k,*fDefault);
break;
default:
invertMatrix(order,k,*fDefault);
break;
}
return *fDefault;
}
Matrix4 InertiaAlign::inverseTransform(Matrix4 transformToInvert)
{
Matrix3 rotationToInvert;
Matrix3 rotationInverted;
Vector3 Translation;
Matrix4 transformInverted;
sout << "Before inversion"<< sendl;
for(unsigned int i=0;i<4;i++)
{
for(unsigned int j=0;j<4;j++)
{
sout << transformToInvert(i,j)<< " ";
}
sout << sendl;
}
for(unsigned int i=0;i<3;i++)
{
for(unsigned int j=0;j<3;j++)
{
rotationToInvert(i,j) = transformToInvert(i,j);
}
Translation(i)=transformToInvert(i,3);
}
bool bS = invertMatrix(rotationInverted,rotationToInvert);
sout << "Translation = " << Translation;
if(!bS)
{
sout <<"Error : Source transformation matrix is not invertible"<<sendl;
}
else //Compute R-1 * t
{
Translation = (-1*rotationInverted * Translation);
}
sout << "Translation = " << Translation;
for(unsigned int i=0;i<3;i++)
{
for(unsigned int j=0;j<3;j++)
{
transformInverted(i,j) = rotationInverted(i,j);
}
transformInverted(i,3)= Translation(i);
}
transformInverted(3,3)=1;
sout << "After inversion"<< sendl;
for(unsigned int i=0;i<4;i++)
{
for(unsigned int j=0;j<4;j++)
{
sout << transformInverted(i,j)<< " ";
}
sout << sendl;
}
return transformInverted;
}
void
TransformCommand::undo()
{
if (!cloud_ptr_)
return;
float transform_matrix_inv[MATRIX_SIZE];
invertMatrix(transform_matrix_, transform_matrix_inv);
float x,y,z;
unsigned int index = 0;
Selection::const_iterator it;
for(it = internal_selection_ptr_ -> begin();
it != internal_selection_ptr_-> end(); ++it)
{
Point3D pt;
index = *it;
pt.x = (*cloud_ptr_)[index].x - cloud_center_[X];
pt.y = (*cloud_ptr_)[index].y - cloud_center_[Y];
pt.z = (*cloud_ptr_)[index].z - cloud_center_[Z];
x = pt.x * cloud_matrix_[0] +
pt.y * cloud_matrix_[4] +
pt.z * cloud_matrix_[8] + cloud_matrix_[12];
y = pt.x * cloud_matrix_[1] +
pt.y * cloud_matrix_[5] +
pt.z * cloud_matrix_[9] + cloud_matrix_[13];
z = pt.x * cloud_matrix_[2] +
pt.y * cloud_matrix_[6] +
pt.z * cloud_matrix_[10] + cloud_matrix_[14];
pt.x = x - translate_x_;
pt.y = y - translate_y_;
pt.z = z - translate_z_;
x = pt.x * transform_matrix_inv[0] +
pt.y * transform_matrix_inv[4] +
pt.z * transform_matrix_inv[8] + transform_matrix_inv[12];
y = pt.x * transform_matrix_inv[1] +
pt.y * transform_matrix_inv[5] +
pt.z * transform_matrix_inv[9] + transform_matrix_inv[13];
z = pt.x * transform_matrix_inv[2] +
pt.y * transform_matrix_inv[6] +
pt.z * transform_matrix_inv[10] + transform_matrix_inv[14];
pt.x = x * cloud_matrix_inv_[0] +
y * cloud_matrix_inv_[4] +
z * cloud_matrix_inv_[8] + cloud_matrix_inv_[12];
pt.y = x * cloud_matrix_inv_[1] +
y * cloud_matrix_inv_[5] +
z * cloud_matrix_inv_[9] + cloud_matrix_inv_[13];
pt.z = x * cloud_matrix_inv_[2] +
y * cloud_matrix_inv_[6] +
z * cloud_matrix_inv_[10] + cloud_matrix_inv_[14];
(*cloud_ptr_)[index].x = pt.x + cloud_center_[X];
(*cloud_ptr_)[index].y = pt.y + cloud_center_[Y];
(*cloud_ptr_)[index].z = pt.z + cloud_center_[Z];
}
}
lfPoint2Dd_t lfTransformPoint2D_inv(lfTransMat2D_t M, lfPoint2Dd_t pi)
{
lfPoint2Dd_t po;
lfTransMat2D_t invM;
invertMatrix(M, invM);
po = lfTransformPoint2D(invM, pi);
return po;
}
/* display cube */
void updateCube(pCube cube,pMesh mesh) {
pScene sc;
pTransform cubetr,view;
GLfloat inv[16],axis[4],trans[4];
int idw;
/* default */
if ( ddebug ) printf("updateCube\n");
/* retrieve context */
idw = currentScene();
sc = cv.scene[idw];
view = sc->view;
cubetr = cube->cubetr;
/* compute cube transform */
if ( cube->active & C_EDIT ) {
invertMatrix(view->matrix,inv);
inv[3] = inv[7] = inv[11] = 0.0f;
inv[15] = 1.0;
/* rotation cumulative */
if ( cubetr->angle != 0.0 ) {
transformVector(trans,cubetr->axis,inv);
glPushMatrix();
glLoadIdentity();
glRotatef(cubetr->angle,trans[0],trans[1],trans[2]);
glMultMatrixf(cubetr->rot);
glGetFloatv(GL_MODELVIEW_MATRIX,cubetr->rot);
glPopMatrix();
}
/* translation cumulative */
axis[0] = cubetr->panx;
axis[1] = cubetr->pany;
axis[2] = 0.0;
axis[3] = 1.0;
transformVector(trans,axis,inv);
cubetr->tra[12] = trans[0];
cubetr->tra[13] = trans[1];
cubetr->tra[14] = trans[2];
/* final transformation */
glPushMatrix();
glLoadIdentity();
glMultMatrixf(cubetr->tra);
glMultMatrixf(cubetr->rot);
glGetFloatv(GL_MODELVIEW_MATRIX,cubetr->matrix);
glPopMatrix();
}
if ( !cubetr->manim ) {
cubetr->angle = 0.0;
cube->active ^= C_UPDATE;
}
}
static void
getMatrix()
{
glGetDoublev(GL_MODELVIEW_MATRIX,_matrix);
/*printf("{ %f %f %f %f }\n %f %f %f %f }\n %f %f %f %f }\n %f %f %f %f }\n",
_matrix[0], _matrix[4], _matrix[8], _matrix[12],
_matrix[1], _matrix[5], _matrix[9], _matrix[13],
_matrix[2], _matrix[6], _matrix[10], _matrix[14],
_matrix[3], _matrix[7], _matrix[11], _matrix[15]);*/
invertMatrix(_matrix,_matrixInverse);
}
Mat4x4f Compass::cameraRotationMatrix(const Camera& camera) const {
Mat4x4f rotation;
rotation[0] = camera.right();
rotation[1] = camera.direction();
rotation[2] = camera.up();
bool invertible = true;
invertMatrix(rotation, invertible);
assert(invertible);
return rotation;
}
void
Nonlinear3D::computePolarDecomposition( const ColumnMajorMatrix & Fhat )
{
// From Rashid, 1993.
ColumnMajorMatrix Fhat_inverse;
invertMatrix( Fhat, Fhat_inverse );
Fhat_inverse = Fhat;
/*Moose::out << "Fhat = " << std::endl;
ColumnMajorMatrix out(Fhat);
out.print();
Moose::out << "Fhat_inverse = " << std::endl;
out = Fhat_inverse;
out.print();*/
const Real Uxx = Fhat_inverse(0,0);
const Real Uxy = Fhat_inverse(0,1);
const Real Uxz = Fhat_inverse(0,2);
const Real Uyx = Fhat_inverse(1,0);
const Real Uyy = Fhat_inverse(1,1);
const Real Uyz = Fhat_inverse(1,2);
const Real Uzx = Fhat_inverse(2,0);
const Real Uzy = Fhat_inverse(2,1);
const Real Uzz = Fhat_inverse(2,2);
const Real Ax = Uyz - Uzy;
const Real Ay = Uzx - Uxz;
const Real Az = Uxy - Uyx;
const Real Q = 0.25 * (Ax*Ax + Ay*Ay + Az*Az);
const Real traceF = Uxx + Uyy + Uzz;
const Real P = 0.25 * (traceF - 1) * (traceF - 1);
const Real Y = 1 / ((Q+P)*(Q+P)*(Q+P));
const Real C1 = std::sqrt(P * (1 + (P*(Q+Q+(Q+P))) * (1-(Q+P)) * Y));
const Real C2 = 0.125 + Q * 0.03125 * (P*P - 12*(P-1)) / (P*P);
const Real C3 = 0.5 * std::sqrt( (P*Q*(3-Q) + P*P*P + Q*Q) * Y );
// Since the input to this routine is the incremental deformation gradient
// and not the inverse incremental gradient, this result is the transpose
// of the one in Rashid's paper.
_incremental_rotation(0,0) = C1 + (C2*Ax)*Ax;
_incremental_rotation(0,1) = (C2*Ay)*Ax + (C3*Az);
_incremental_rotation(0,2) = (C2*Az)*Ax - (C3*Ay);
_incremental_rotation(1,0) = (C2*Ax)*Ay - (C3*Az);
_incremental_rotation(1,1) = C1 + (C2*Ay)*Ay;
_incremental_rotation(1,2) = (C2*Az)*Ay + (C3*Ax);
_incremental_rotation(2,0) = (C2*Ax)*Az + (C3*Ay);
_incremental_rotation(2,1) = (C2*Ay)*Az - (C3*Ax);
_incremental_rotation(2,2) = C1 + (C2*Az)*Az;
}
void setLightMatrixNoBias(double *result, double *shadowProjection, double *shadowModelView, double* camModelView) {
/*Matrix44f shadowTransMatrix = mShadowCam.getProjectionMatrix();
shadowTransMatrix *= mShadowCam.getModelViewMatrix();
shadowTransMatrix *= camera.getInverseModelViewMatrix();
return shadowTransMatrix;*/
double trans2[16];
multMatrix(trans2,shadowProjection,shadowModelView);
//setEqual(trans, shadowModelView);
double inv[16];
invertMatrix(inv, camModelView);
multMatrix(result, trans2, inv);
}
TransformCommand::TransformCommand(ConstSelectionPtr selection_ptr,
CloudPtr cloud_ptr,
const float *matrix,
float translate_x,
float translate_y,
float translate_z)
: selection_ptr_(selection_ptr), cloud_ptr_(cloud_ptr),
translate_x_(translate_x), translate_y_(translate_y),
translate_z_(translate_z)
{
internal_selection_ptr_ = SelectionPtr(new Selection(*selection_ptr));
std::copy(matrix, matrix+MATRIX_SIZE, transform_matrix_);
const float *cloud_matrix = cloud_ptr_->getMatrix();
std::copy(cloud_matrix, cloud_matrix+MATRIX_SIZE, cloud_matrix_);
invertMatrix(cloud_matrix, cloud_matrix_inv_);
cloud_ptr_->getCenter(cloud_center_[X], cloud_center_[Y], cloud_center_[Z]);
}
// Screen Space
void ScreenProject::calculateMVP(GLint * vp, double * mv, double * p)
{
vp_[0] = vp[0];
vp_[1] = vp[1];
vp_[2] = vp[2];
vp_[3] = vp[3];
// Matrix Multiplication
for (int k=0; k<16; k+=4)
{
for (int j=0; j<4; j++)
{
mvp_[j+k] = 0;
for (int i=0; i<4; i++)
mvp_[j+k] += p[4*i+j]*mv[i+k];
}
}
invertMatrix(mvp_, mvpInv_);
}
void
Element::polarDecompositionEigen( const ColumnMajorMatrix & Fhat, ColumnMajorMatrix & Rhat, SymmTensor & strain_increment )
{
const int ND = 3;
ColumnMajorMatrix eigen_value(ND,1), eigen_vector(ND,ND);
ColumnMajorMatrix invUhat(ND,ND), logVhat(ND,ND);
ColumnMajorMatrix n1(ND,1), n2(ND,1), n3(ND,1), N1(ND,1), N2(ND,1), N3(ND,1);
ColumnMajorMatrix Chat = Fhat.transpose() * Fhat;
Chat.eigen(eigen_value,eigen_vector);
for(int i = 0; i < ND; i++)
{
N1(i) = eigen_vector(i,0);
N2(i) = eigen_vector(i,1);
N3(i) = eigen_vector(i,2);
}
const Real lamda1 = std::sqrt(eigen_value(0));
const Real lamda2 = std::sqrt(eigen_value(1));
const Real lamda3 = std::sqrt(eigen_value(2));
const Real log1 = std::log(lamda1);
const Real log2 = std::log(lamda2);
const Real log3 = std::log(lamda3);
ColumnMajorMatrix Uhat = N1 * N1.transpose() * lamda1 + N2 * N2.transpose() * lamda2 + N3 * N3.transpose() * lamda3;
invertMatrix(Uhat,invUhat);
Rhat = Fhat * invUhat;
strain_increment = N1 * N1.transpose() * log1 + N2 * N2.transpose() * log2 + N3 * N3.transpose() * log3;
}
void iterativeSolvers_unitTest() {
// small matrix test
{
Vector4f test = makeVector4f(1.0f, 3.14f, 2.0f, 2.71f);
Matrix4f mat = makeRotX4f(0.32f) * makeRotY4f(-0.39f) * makeRotZ4f(0.76f);
mat += mat.transpose();
mat += IDENTITY4F*5;
Vector4f rhs = mat * test;
Matrix4f invMat = invertMatrix(mat);
Vector4f directFloat = invMat * rhs;
DVectorD testD(4); for (card32 i=0; i<4; i++) testD[i] = test[i];
DVectorD rhsD(4); for (card32 i=0; i<4; i++) rhsD[i] = rhs[i];
SparseMatrixD matS(4);
for (card32 r=0; r<4; r++) {
for (card32 c=0; c<4; c++) {
double v = mat[c][r];
if (v != 0) {
matS[r].setEntry(c, v);
}
}
}
output << "Matrix:\n" << mat.toString() << "\n";
output << "true solution: " << test.toString() << "\n";
output << "right hand side: " << rhs.toString() << "\n";
output << "direct float prec. solution: " << directFloat.toString() << "\n\n";
output << "Matrix:\n" << matS.toString() << "\n";
output << "true solution: " << testD.toString() << "\n";
output << "right hand side: " << rhsD.toString() << "\n\n";
DVectorD x = nullDVector<double>(4);
card32 numIterations = 1000;
gaussSeidelSolve(matS, x, rhsD, numIterations, 1E-20, 1E-5, false);
output << "Gauss-Seidel solution: " << x.toString() << "\n\n";
numIterations = 1000;
x = nullDVector<double>(4);
steepestDescentSolve(matS, x, rhsD, numIterations, 1E-5, false);
output << "Steepest-Descent solution: " << x.toString() << "\n\n";
numIterations = 1000;
x = nullDVector<double>(4);
conjugateGradientsSolve(matS, x, rhsD, numIterations, 1E-5, true);
output << "CG solution: " << x.toString() << "\n\n";
}
// large matrix test
{
SparseMatrixD mat(MDIM*MDIM);
for (int y=0; y<MDIM; y++) {
for (int x=0; x<MDIM; x++) {
addEntry(mat, x,y, x,y, -4, MDIM);
addEntry(mat, x,y, x+1,y, 1, MDIM);
addEntry(mat, x,y, x-1,y, 1, MDIM);
addEntry(mat, x,y, x,y+1, 1, MDIM);
addEntry(mat, x,y, x,y-1, 1, MDIM);
}
}
Timer T; card32 vt;
DVectorD x = nullDVector<double>(MDIM*MDIM);
DVectorD b = scalarDVector<double>(MDIM*MDIM, 1);
DVectorD gs, sd, cg;
x = nullDVector<double>(MDIM*MDIM);
T.getDeltaValue();
card32 numIterations = 100000;
conjugateGradientsSolve(mat, x, b, numIterations, 1E-7, false);
vt = T.getDeltaValue();
output << "CG time: " << vt << "\n\n";
output << "Large sytem CG solution (LAPL(f) == 1):\n";
for (int y=0; y<MDIM; y++) {
for (int xp=0; xp<MDIM; xp++) {
output << x[xp +y*MDIM] << "\t";
}
output.cr();
}
output.cr();
cg = x;
x = nullDVector<double>(MDIM*MDIM);
T.getDeltaValue();
numIterations = 100000;
gaussSeidelSolve(mat, x, b, numIterations, 1E-20, 1E-7, false);
vt = T.getDeltaValue();
output << "GS time: " << vt << "\n\n";
output << "Large sytem Gauss-Seidel solution (LAPL(f) == 1):\n";
for (int y=0; y<MDIM; y++) {
for (int xp=0; xp<MDIM; xp++) {
output << x[xp +y*MDIM] << "\t";
}
output.cr();
}
output.cr();
gs = x;
x = nullDVector<double>(MDIM*MDIM);
T.getDeltaValue();
numIterations = 100000;
steepestDescentSolve(mat, x, b, numIterations, 1E-7, false);
vt = T.getDeltaValue();
output << "SD time: " << vt << "\n\n";
output << "Large sytem steepest descent solution (LAPL(f) == 1):\n";
for (int y=0; y<MDIM; y++) {
for (int xp=0; xp<MDIM; xp++) {
output << (long double)x[xp +y*MDIM] << "\t";
}
output.cr();
}
output.cr();
sd = x;
x = nullDVector<double>(MDIM*MDIM);
output << "|gs-sd|_2 " << (long double)norm(gs-sd) << "\n";
output << "|gs-cg|_2 " << (long double)norm(gs-cg) << "\n";
vector< DVectorD > eigenVects;
vector< double > eigenvalues;
DVectorD eigenvector;
double eigenvalue;
for (int i=0; i<10; i++) {
powerIteration(mat, eigenVects, eigenvector, eigenvalue, 1.0001, 1000, false);
eigenVects.push_back(eigenvector);
eigenvalues.push_back(eigenvalue);
}
for (int i=0; i<10; i++) {
output << i << " largest of large sytem eigenvector (LAPL(f) == 1):\n";
for (int y=0; y<MDIM; y++) {
for (int xp=0; xp<MDIM; xp++) {
output << eigenVects[i][xp +y*MDIM] << "\t";
}
output.cr();
}
output.cr();
}
output << "eigenvalue:\n";
for (int i=0; i<10; i++) {
output << eigenvalues[i] << "\n";
}
T.getDeltaValue();
eigenVects.clear();
for (int i=0; i<10; i++) {
GaussSeidelSolver<double> linSolve;
inversePowerIteration(mat, eigenVects, eigenvector, eigenvalue, &linSolve, 1.0001, 1000, false);
eigenvalues.push_back(eigenvalue);
eigenVects.push_back(eigenvector);
}
output << "GS-PowerIt time: " << T.getDeltaValue() << "\n\n";
eigenVects.clear();
for (int i=0; i<10; i++) {
CGSolver<double> linSolve;
inversePowerIteration(mat, eigenVects, eigenvector, eigenvalue, &linSolve, 1.0001, 1000, false);
eigenvalues.push_back(eigenvalue);
eigenVects.push_back(eigenvector);
}
output << "CG-PowerIt time: " << T.getDeltaValue() << "\n\n";
eigenVects.clear();
for (int i=0; i<10; i++) {
SteepestDescentSolver<double> linSolve;
inversePowerIteration(mat, eigenVects, eigenvector, eigenvalue, &linSolve, 1.0001, 1000, false);
eigenvalues.push_back(eigenvalue);
eigenVects.push_back(eigenvector);
}
output << "Steepest descent-PowerIt time: " << T.getDeltaValue() << "\n\n";
for (int i=0; i<10; i++) {
output << i << " smallest of large sytem eigenvector (LAPL(f) == 1):\n";
for (int y=0; y<MDIM; y++) {
for (int xp=0; xp<MDIM; xp++) {
output << eigenVects[i][xp +y*MDIM] << "\t";
}
output.cr();
}
output.cr();
}
output << "eigenvalues:\n";
for (int i=0; i<10; i++) {
output << eigenvalues[i] << "\n";
}
}
{
Timer T;
output << "cg scaling test\n";
for (unsigned i=1; i<21; i++) {
const int size = i * 10;
SparseMatrixD mat(size*size);
for (int y=0; y<size; y++) {
for (int x=0; x<size; x++) {
addEntry(mat, x,y, x,y, -4, size);
addEntry(mat, x,y, x+1,y, 1, size);
addEntry(mat, x,y, x-1,y, 1, size);
addEntry(mat, x,y, x,y+1, 1, size);
addEntry(mat, x,y, x,y-1, 1, size);
}
}
Timer T; card32 vt;
DVectorD x = nullDVector<double>(size*size);
DVectorD b = scalarDVector<double>(size*size, 1);
T.getDeltaValue();
card32 numIterations = 100000;
conjugateGradientsSolve(mat, x, b, numIterations, 1E-7, false);
vt = T.getDeltaValue();
output << i << "\t" << vt << "\n";
}
}
{
Timer T;
output << "gs scaling test\n";
for (unsigned i=1; i<21; i++) {
const int size = i * 10;
SparseMatrixD mat(size*size);
for (int y=0; y<size; y++) {
for (int x=0; x<size; x++) {
addEntry(mat, x,y, x,y, -4, size);
addEntry(mat, x,y, x+1,y, 1, size);
addEntry(mat, x,y, x-1,y, 1, size);
addEntry(mat, x,y, x,y+1, 1, size);
addEntry(mat, x,y, x,y-1, 1, size);
}
}
Timer T; card32 vt;
DVectorD x = nullDVector<double>(size*size);
DVectorD b = scalarDVector<double>(size*size, 1);
T.getDeltaValue();
card32 numIterations = 100000;
gaussSeidelSolve(mat, x, b, numIterations, 1E-20, 1E-7, false);
vt = T.getDeltaValue();
output << i << "\t" << vt << "\n";
}
}
{
Timer T;
output << "sd scaling test\n";
for (unsigned i=1; i<21; i++) {
const int size = i * 10;
SparseMatrixD mat(size*size);
for (int y=0; y<size; y++) {
for (int x=0; x<size; x++) {
addEntry(mat, x,y, x,y, -4, size);
addEntry(mat, x,y, x+1,y, 1, size);
addEntry(mat, x,y, x-1,y, 1, size);
addEntry(mat, x,y, x,y+1, 1, size);
addEntry(mat, x,y, x,y-1, 1, size);
}
}
Timer T; card32 vt;
DVectorD x = nullDVector<double>(size*size);
DVectorD b = scalarDVector<double>(size*size, 1);
T.getDeltaValue();
card32 numIterations = 100000;
steepestDescentSolve(mat, x, b, numIterations, 1E-7, false);
vt = T.getDeltaValue();
output << i << "\t" << vt << "\n";
}
}
}
int
iterateDynamics(std::vector<Robot *> robotVec,
std::vector<DynamicBody *> bodyVec,
DynamicParameters *dp)
{
double h = dp->timeStep;
bool useContactEps = dp->useContactEps;
static double Jcg_tmp[9],Jcg_B[9],Jcg_N[9],Jcg_N_inv[9],R_N_B[9];
static double db0=0.0,tmp3[3];
static mat3 Rot;
static int info;
World *myWorld;
KinematicChain *chain;
int numBodies = bodyVec.size(),errCode = 0;
int numRobots = robotVec.size();
int numJoints=0;
int numDOF=0;
int bn,cn,i,j;
int Mrows,Dcols,Arows,Hrows,Hcols,Nurows,Nucols;
int numDOFLimits=0;
std::list<Contact *> contactList;
std::list<Contact *> objContactList;
std::list<Contact *>::iterator cp;
// unsigned long dmark = dmalloc_mark();
double *ql = new double[7*numBodies];
double *qnew = new double[7*numBodies];
double *vl = new double[6*numBodies];
double *vlnew = new double[6*numBodies];
double *M = new double[(6*numBodies)*(6*numBodies)];
double *M_i = new double[(6*numBodies)*(6*numBodies)];
double *fext = new double[6*numBodies];
// LCP matrix
double *A;
// LCP vectors
double *g,*lambda;
double *predLambda = NULL; //used for debugging the prediction of LCP basis
// main matrices for contact constraints
double *H;
// main matrices for joint constraints
double *Nu;
// main vector for contact constraints
double *k;
// main vectors for joint constraints
double *eps;
// intermediate matrices for contact constraints
double *HtM_i,*v1;
// intermediate matrices for contact constraints
double *v2;
// intermediate matrices for case of both joint and contact constraints
double *NutM_i,*NutM_iNu,*INVNutM_iNu,*INVNutM_iNuNut;
double *INVNutM_iNuNutM_i,*INVNutM_iNuNutM_iH;
// intermediate vectors for case of both joint and contact constraints
double *NutM_ikminuseps,*INVNutM_iNuNutM_ikminuseps;
double *currq,*currM;
Mrows = 6*numBodies;
myWorld = bodyVec[0]->getWorld();
std::map<Body*, int> islandIndices;
for (i=0;i<myWorld->getNumBodies();i++) {
islandIndices.insert( std::pair<Body*, int>(myWorld->getBody(i), -1) );
}
for (i=0;i<numBodies;i++) {
islandIndices[ bodyVec[i] ] = i;
}
// count the joints and DOF, and the joint coupling constraints
int numCouplingConstraints = 0;
for (i=0;i<numRobots;i++) {
numDOF += robotVec[i]->getNumDOF();
for (j=0;j<robotVec[i]->getNumChains();j++) {
chain = robotVec[i]->getChain(j);
numJoints += chain->getNumJoints();
}
for (j=0;j<robotVec[i]->getNumDOF();j++) {
numCouplingConstraints += robotVec[i]->getDOF(j)->getNumCouplingConstraints();
numDOFLimits += robotVec[i]->getDOF(j)->getNumLimitConstraints();
}
}
DBGP("Dynamics time step: " << h);
DBGP("numJoints: " << numJoints);
// count the total number of joints and contacts
int numContacts = 0;
int numTotalFrictionEdges = 0;
int numDynJointConstraints=0;
for (bn=0;bn<numBodies;bn++) {
//count joints
if (bodyVec[bn]->getDynJoint()) {
int numCon = bodyVec[bn]->getDynJoint()->getNumConstraints();
numDynJointConstraints += numCon;
DBGP(bodyVec[bn]->getName().latin1() << ": " << numCon << " constraints");
}
//count contacts
objContactList = bodyVec[bn]->getContacts();
for (cp=objContactList.begin();cp!=objContactList.end();cp++) {
// check if the mate of this contact is already in the contact list
if (std::find(contactList.begin(),contactList.end(),(*cp)->getMate()) == contactList.end()) {
numContacts++;
numTotalFrictionEdges += (*cp)->numFrictionEdges;
contactList.push_back(*cp);
}
}
}
DBGP("Num contacts: " << numContacts);
DBGP("Num friction edges: " << numTotalFrictionEdges);
DBGP("Num dynjoint: " << numDynJointConstraints);
// zero out matrices
dcopy(Mrows*Mrows,&db0,0,M,1);
dcopy(Mrows*Mrows,&db0,0,M_i,1);
dcopy(Mrows,&db0,0,fext,1);
//allocate the joint constraint matrices
if (numJoints) {
Nurows = Mrows;
Nucols = numDynJointConstraints + numCouplingConstraints;
DBGP("Nucols: " << Nucols);
Nu = new double[Nurows * Nucols];
dcopy(Nurows*Nucols,&db0,0,Nu,1);
eps = new double[Nucols];
dcopy(Nucols,&db0,0,eps,1);
Arows = Mrows+Nucols;
}
// allocate the LCP matrix
if (numContacts || numDOFLimits) {
Dcols = numTotalFrictionEdges;
DBGP("numContacts " << numContacts);
DBGP("Dcols " << Dcols);
DBGP("numDOFLimits " << numDOFLimits);
Hrows = Mrows;
Hcols = Dcols + 2*numContacts + numDOFLimits;
H = new double[Hrows * Hcols];
dcopy(Hrows*Hcols,&db0,0,H,1);
v1 = new double[Hrows * Hcols];
v2 = new double[Hrows];
dcopy(Hrows*Hcols,&db0,0,v1,1);
dcopy(Hrows,&db0,0,v2,1);
k = new double[Mrows]; //holds mass*previous velocity and external impulses
Arows = Hcols;
lambda = new double[Arows]; // the LCP solution
} else {
Dcols = 0;
}
// allocate the constraint matrix
if (numJoints || numContacts) {
A = new double[Arows*Arows];
g = new double[Arows];
dcopy(Arows*Arows,&db0,0,A,1);
dcopy(Arows,&db0,0,g,1);
}
// compute mass matrix and external forces
for (bn=0;bn<numBodies;bn++) {
memcpy(vl+6*bn,bodyVec[bn]->getVelocity(),6*sizeof(double));
memcpy(vlnew+6*bn,bodyVec[bn]->getVelocity(),6*sizeof(double));
memcpy(ql+7*bn,bodyVec[bn]->getPos(),7*sizeof(double));
memcpy(qnew+7*bn,bodyVec[bn]->getPos(),7*sizeof(double));
currq = qnew + 7*bn;
Quaternion tmpQuat(currq[3],currq[4],currq[5],currq[6]);
tmpQuat.ToRotationMatrix(Rot);
// The rotation matrix returned by ToRotationMatrix is expressed as
// a graphics style rot matrix (new axes are in rows), the R_N_B matrix
// is a robotics style rot matrix (new axes in columns)
R_N_B[0] = Rot[0]; R_N_B[3] = Rot[1]; R_N_B[6] = Rot[2];
R_N_B[1] = Rot[3]; R_N_B[4] = Rot[4]; R_N_B[7] = Rot[5];
R_N_B[2] = Rot[6]; R_N_B[5] = Rot[7]; R_N_B[8] = Rot[8];
// Jcg_N = R_N_B * Jcg_B * R_N_B';
// where Jcg_B is inertia matrix in body coords
// Jcg_N is inertia matrix in world coords ?
memcpy(Jcg_B,bodyVec[bn]->getInertia(),9*sizeof(double));
//multiply by mass
dscal(9, bodyVec[bn]->getMass(), Jcg_B, 1);
dgemm("N","N",3,3,3,1.0,R_N_B,3,Jcg_B,3,0.0,Jcg_tmp,3);
dgemm("N","T",3,3,3,1.0,Jcg_tmp,3,R_N_B,3,0.0,Jcg_N,3);
if ((info = invertMatrix(3,Jcg_N,Jcg_N_inv))) {
printf("In iterateDynamics, inertia matrix inversion failed (info is %d)\n",info);
fprintf(stderr,"%f %f %f\n",Jcg_B[0], Jcg_B[1], Jcg_B[2]);
fprintf(stderr,"%f %f %f\n",Jcg_B[3], Jcg_B[4], Jcg_B[5]);
fprintf(stderr,"%f %f %f\n",Jcg_B[6], Jcg_B[7], Jcg_B[8]);
fprintf(stderr,"Body is %s\n",bodyVec[bn]->getName().latin1());
}
currM = M+((6*bn)*Mrows + bn*6); //point to the correct block of M
currM[0] = bodyVec[bn]->getMass();
currM[6*numBodies+1] = bodyVec[bn]->getMass();
currM[12*numBodies+2] = bodyVec[bn]->getMass();
fillMatrixBlock(Jcg_N,3,3,3,5,5,currM,Mrows);
currM = M_i+((6*bn)*Mrows + bn*6);//point to correct block of M_i
currM[0] = 1.0/bodyVec[bn]->getMass();
currM[Mrows+1] = 1.0/bodyVec[bn]->getMass();
currM[2*Mrows+2] = 1.0/bodyVec[bn]->getMass();
fillMatrixBlock(Jcg_N_inv,3,3,3,5,5,currM,Mrows);
// compute external wrench
// fext = [ 0 0 -9810.0*mass -[ang_vel_N x (Jcg_N * ang_vel_N)] ]
//based on this, it would appear that graspit force units are N*1.0e6
fext[6*bn+2] = -9810.0 * bodyVec[bn]->getMass() * dp->gravityMultiplier; // force of gravity
// fext[6*bn+2] = 0; // NO force of gravity
dgemv("N",3,3,1.0,Jcg_N,3,&vl[6*bn+3],1,0.0,tmp3,1); // inertial moments
fext[6*bn+3] = - (vl[6*bn+4]*tmp3[2] - vl[6*bn+5]*tmp3[1]);
fext[6*bn+4] = - (vl[6*bn+5]*tmp3[0] - vl[6*bn+3]*tmp3[2]);
fext[6*bn+5] = - (vl[6*bn+3]*tmp3[1] - vl[6*bn+4]*tmp3[0]);
double ForcesToBodyFrame[36];
transf invBody = bodyVec[bn]->getTran().inverse();
vec3 invBodyTransl = invBody.translation();
buildForceTransform(invBody,invBodyTransl,ForcesToBodyFrame);
DBGP("fext initial: ");
DBGST( disp_mat(stdout,&fext[6*bn],1,6,0) );
// add any other wrenches that have accumulated on the body
daxpy(6,1.0,bodyVec[bn]->getExtWrenchAcc(),1,&fext[6*bn],1);
DBGP("fext with accumulated wrench: ");
DBGST( disp_mat(stdout,&fext[6*bn],1,6,0) );
if (numContacts||numDOFLimits) {
// k = Mv_l + hfext
currM = M+((6*bn)*Mrows + bn*6); //point to the correct block of M
dgemv("N",6,6,1.0,currM,Mrows,vl+6*bn,1,0.0,k+6*bn,1);
}
}
if (numJoints) {
int ncn = 0;
int hcn = 0;
for (i=0;i<numBodies;i++) {
if (bodyVec[i]->getDynJoint())
bodyVec[i]->getDynJoint()-> buildConstraints(Nu,eps,numBodies,islandIndices,ncn);
}
for (i=0;i<numRobots;i++) {
robotVec[i]->buildDOFLimitConstraints(islandIndices,numBodies,H,g,hcn);
robotVec[i]->buildDOFCouplingConstraints(islandIndices,numBodies,Nu,eps,ncn);
}
for (i=0;i<Nucols;i++) {
eps[i] *= ERP/h;
}
for (i=0; i<hcn; i++) {
g[i] *= ERP/h;
}
}
// add contacts to the LCP
if (!contactList.empty()) {
DBGP("processing contacts");
double Ftform_N_C[36];
// A is square
double *Wn = &H[numDOFLimits*Hrows];
double *D = &H[(numDOFLimits+numContacts)*Hrows];
double *E = &A[(numDOFLimits+numContacts+Dcols)*Arows + numDOFLimits+numContacts];
double *negET = &A[(numDOFLimits+numContacts)*Arows + numDOFLimits+numContacts+Dcols];
double *MU = &A[numDOFLimits*Arows + numDOFLimits+numContacts+Dcols];
double *contactEps = &g[numDOFLimits];
int frictionEdgesCount = 0;
for (cp=contactList.begin(),cn=0; cp!=contactList.end(); cp++,cn++){
//DBGP("contact " << cn);
transf cf = (*cp)->getContactFrame() * (*cp)->getBody1Tran();
transf cf2 = (*cp)->getMate()->getContactFrame() * (*cp)->getBody2Tran();
DBGP("CONTACT DISTANCE: " << (cf.translation() - cf2.translation()).len());
if (useContactEps) {
contactEps[cn] = MIN(0.0,-ERP/h *
(Contact::THRESHOLD/2.0 - (cf.translation() - cf2.translation()).len()));
}
DBGP(" EPS: " << contactEps[cn]);
vec3 normal(cf.affine().element(2,0), cf.affine().element(2,1), cf.affine().element(2,2));
// find which body is this contact from
for (bn=0;bn<numBodies;bn++)
if ((*cp)->getBody1() == bodyVec[bn]) break;
if (bn<numBodies) {
//????? this doesn't seem correct
vec3 radius = cf.translation() - ( bodyVec[bn]->getCoG() * (*cp)->getBody1Tran() - position::ORIGIN );
// radius = radius / 1000.0; // convert to meters
vec3 RcrossN = radius * normal;
DBGP("body1 normal: " << normal);
DBGP("body1 radius: " << radius);
Wn[cn*Hrows+6*bn] = normal.x();
Wn[cn*Hrows+6*bn+1] = normal.y();
Wn[cn*Hrows+6*bn+2] = normal.z();
Wn[cn*Hrows+6*bn+3] = RcrossN.x();
Wn[cn*Hrows+6*bn+4] = RcrossN.y();
Wn[cn*Hrows+6*bn+5] = RcrossN.z();
vec3 bodyOrigin = bodyVec[bn]->getCoG() * (*cp)->getBody1Tran() - position::ORIGIN;
buildForceTransform(cf,bodyOrigin,Ftform_N_C);
/* dgemm("N","N", 6,Contact::numFrictionEdges,6, 1.0,Ftform_N_C,6, Contact::frictionEdges,6,
0.0,&D[Contact::numFrictionEdges*cn*Hrows+6*bn],Hrows); */
dgemm("N","N",
6,(*cp)->numFrictionEdges,6, //m, n, k
1.0,Ftform_N_C,6, //alfa, A, lda
(*cp)->frictionEdges,6, //B, ldb
0.0,&D[ frictionEdgesCount*Hrows+6*bn],Hrows); //beta, C, ldc
}
//find the other body
for(bn=0;bn<numBodies;bn++)
if ((*cp)->getBody2() == bodyVec[bn]) break;
if (bn<numBodies) {
//normal = vec3(cf2.affine().element(2,0), cf2.affine().element(2,1),cf2.affine().element(2,2));
normal = -normal;
//vec3 radius = cf2.translation() - (bodyVec[bn]->getCoG() * (*cp)->getBody2Tran() - position::ORIGIN);
vec3 radius = cf.translation() - (bodyVec[bn]->getCoG() * (*cp)->getBody2Tran() - position::ORIGIN);
vec3 RcrossN = radius * normal;
DBGP("body2 normal: " << normal);
DBGP("body2 radius: " << radius);
Wn[cn*Hrows+6*bn] = normal.x();
Wn[cn*Hrows+6*bn+1] = normal.y();
Wn[cn*Hrows+6*bn+2] = normal.z();
Wn[cn*Hrows+6*bn+3] = RcrossN.x();
Wn[cn*Hrows+6*bn+4] = RcrossN.y();
Wn[cn*Hrows+6*bn+5] = RcrossN.z();
vec3 bodyOrigin = bodyVec[bn]->getCoG()*(*cp)->getBody2Tran() - position::ORIGIN;
buildForceTransform(cf,bodyOrigin,Ftform_N_C);
//buildForceTransform(cf2,bodyOrigin,Ftform_N_C);
/* dgemm("N","N",6,Contact::numFrictionEdges,6,-1.0,Ftform_N_C,6, Contact::frictionEdges,6,
0.0,&D[Contact::numFrictionEdges*cn*Hrows+6*bn],Hrows);*/
//original graspit had a -1.0 here in front of Ftform_N_C
dgemm("N","N",
6,(*cp)->numFrictionEdges,6,
-1.0,Ftform_N_C,6,
(*cp)->frictionEdges,6,
0.0,&D[ frictionEdgesCount*Hrows+6*bn ],Hrows);
}
//for (i=cn*Contact::numFrictionEdges; i<(cn+1)*Contact::numFrictionEdges; i++) {
for (i=frictionEdgesCount; i<frictionEdgesCount+(*cp)->numFrictionEdges; i++) {
E[cn*Arows+i] = 1.0;
negET[i*Arows+cn] = -1.0;
}
MU[cn*Arows + cn] = (*cp)->getCof();
frictionEdgesCount += (*cp)->numFrictionEdges;
}
}
if (numContacts || numDOFLimits)
daxpy(Mrows,h,fext,1,k,1);
if (numJoints && (numContacts || numDOFLimits)) {
// Cnu1 = INV(Nu'M_iNu)Nu'M_iH
// Cnu2 = INV(Nu'M_iNu)(Nu'M_ik-eps)
// v1 = -NuCnu1
// v2 = -NuCnu2
NutM_i = new double[Nucols*Mrows];
NutM_iNu = new double[Nucols*Nucols];
INVNutM_iNu = new double[Nucols*Nucols];
INVNutM_iNuNut = new double[Nucols*Nurows];
INVNutM_iNuNutM_i = new double[Nucols*Mrows];
INVNutM_iNuNutM_iH = new double[Nucols*Hcols];
NutM_ikminuseps = new double[Nucols];
INVNutM_iNuNutM_ikminuseps = new double[Nucols];
dgemm("T","N",Nucols,Mrows,Mrows,1.0,Nu,Nurows,M_i,Mrows, 0.0,NutM_i,Nucols);
dgemm("N","N",Nucols,Nucols,Mrows,1.0,NutM_i,Nucols,Nu,Nurows, 0.0,NutM_iNu,Nucols);
if ((info = invertMatrix(Nucols,NutM_iNu,INVNutM_iNu)))
printf("In iterateDynamics, NutM_iNu matrix inversion failed (info is %d)\n",info);
dgemm("N","T",Nucols,Nurows,Nucols,1.0,INVNutM_iNu,Nucols,Nu,Nurows,
0.0,INVNutM_iNuNut,Nucols);
dgemm("N","N",Nucols,Mrows,Mrows,1.0,INVNutM_iNuNut,Nucols,M_i,Mrows,
0.0,INVNutM_iNuNutM_i,Nucols);
dgemm("N","N",Nucols,Hcols,Mrows,1.0,INVNutM_iNuNutM_i,Nucols,H,Hrows,
0.0,INVNutM_iNuNutM_iH,Nucols);
dgemm("N","N",Nurows,Hcols,Nucols,-1.0,Nu,Nurows,INVNutM_iNuNutM_iH,Nucols,
0.0,v1,Nurows);
dgemv("N",Nucols,Mrows,1.0,NutM_i,Nucols,k,1,0.0,NutM_ikminuseps,1);
daxpy(Nucols,-1.0,eps,1,NutM_ikminuseps,1);
dgemv("N",Nucols,Nucols,1.0,INVNutM_iNu,Nucols,NutM_ikminuseps,1,
0.0,INVNutM_iNuNutM_ikminuseps,1);
dgemv("N",Nurows,Nucols,-1.0,Nu,Nurows,INVNutM_iNuNutM_ikminuseps,1,
0.0,v2,1);
}
if (numContacts || numDOFLimits) {
// in the simple case without joint constraints
// A = H'M_iv1 + N
// g = H'M_iv2
// where N is already stored in A
// v1 is the first term of v_(l+1) and v2 is the second term
// v_l+1 = M_i(v1 lambda + v2) = M_i(H lambda + k)
// k is (Mv_l + hfext)
//add H to v1
//add k to v2
DBGP("k:");
DBGST( disp_mat(stdout,k,1,Mrows,0) );
DBGP("first g:");
DBGST( disp_mat(stdout,g,1,Arows,0) );
daxpy(Mrows*Hcols,1.0,H,1,v1,1);
daxpy(Mrows,1.0,k,1,v2,1);
// build A and g
HtM_i = new double[Hcols*Mrows];
dgemm("T","N",Hcols,Mrows,Hrows,1.0,H,Hrows,M_i,Mrows,0.0,HtM_i,Hcols);
dgemm("N","N",Hcols,Hcols,Mrows,1.0,HtM_i,Hcols,v1,Mrows,1.0,A,Arows);
// dgemv("N",Hcols,Mrows,1.0,HtM_i,Hcols,v2,1,0.0,g,1);
dgemv("N",Hcols,Mrows,1.0,HtM_i,Hcols,v2,1,1.0,g,1);
}
int frictionEdgesCount;
//debug information; can be removed
if (numContacts || numDOFLimits) {
bool lemkePredict = false;
if (lemkePredict) {
//try to use information from previous time steps to guess a good starting basis for Lemke's algorithm
assembleLCPPrediction(lambda, Arows, numDOFLimits, &contactList);
predLambda = new double[Arows]; // keep a copy of the prediction so we can check it later
dcopy(Arows, lambda, 1, predLambda, 1);
// fprintf(stderr,"Prediction: \n");
// printLCPBasis(predLambda, Arows, numDOFLimits, numContacts);
}
// double startTime;
// startTime = getTime();
DBGP("g:");
DBGST( for (i=0;i<Arows;i++) printf("%le ",g[i]); );
Matrix3x3d
computeIntersectionCovariance(vector<Matrix3x4d> const& projections,
vector<PointMeasurement> const& measurements,
double sigma)
{
Matrix<double> Cp(3, 3, 0.0);
Cp[0][0] = Cp[1][1] = sigma;
Cp[2][2] = 0.0;
int const N = measurements.size();
Matrix<double> A(2*N, 4, 0.0);
InlineMatrix<double, 2, 3> Sp;
makeZeroMatrix(Sp);
InlineMatrix<double, 2, 4> Sp_P;
for (int i = 0; i < N; ++i)
{
Sp[0][1] = -1; Sp[0][2] = measurements[i].pos[1];
Sp[1][0] = 1; Sp[1][2] = -measurements[i].pos[0];
int const view = measurements[i].view;
multiply_A_B(Sp, projections[view], Sp_P);
A[2*i+0][0] = Sp_P[0][0]; A[2*i+0][1] = Sp_P[0][1]; A[2*i+0][2] = Sp_P[0][2]; A[2*i+0][3] = Sp_P[0][3];
A[2*i+1][0] = Sp_P[1][0]; A[2*i+1][1] = Sp_P[1][1]; A[2*i+1][2] = Sp_P[1][2]; A[2*i+1][3] = Sp_P[1][3];
} // end for (i)
SVD<double> svd(A);
Matrix<double> V;
svd.getV(V);
Vector4d X;
X[0] = V[0][3]; X[1] = V[1][3]; X[2] = V[2][3]; X[3] = V[3][3];
Vector3d P;
Matrix<double> S(2, 3, 0.0);
Matrix<double> B(2*N, 3*N, 0.0);
for (int i = 0; i < N; ++i)
{
int const view = measurements[i].view;
multiply_A_v(projections[view], X, P);
P[0] /= P[2]; P[1] /= P[2]; P[2] = 1.0;
S[0][1] = -P[2]; S[0][2] = P[1];
S[1][0] = P[2]; S[1][2] = -P[0];
B[2*i+0][3*i+0] = -S[0][0]; B[2*i+0][3*i+1] = -S[0][1]; B[2*i+0][3*i+2] = S[0][2];
B[2*i+1][3*i+0] = -S[1][0]; B[2*i+1][3*i+1] = -S[1][1]; B[2*i+1][3*i+2] = S[1][2];
} // end for (i)
Matrix<double> C(3*N, 3*N, 0.0);
for (int i = 0; i < N; ++i)
{
C[3*i+0][3*i+0] = Cp[0][0]; C[3*i+0][3*i+1] = Cp[0][1]; C[3*i+0][3*i+2] = Cp[0][2];
C[3*i+1][3*i+0] = Cp[1][0]; C[3*i+1][3*i+1] = Cp[1][1]; C[3*i+1][3*i+2] = Cp[1][2];
C[3*i+2][3*i+0] = Cp[2][0]; C[3*i+2][3*i+1] = Cp[2][1]; C[3*i+2][3*i+2] = Cp[2][2];
} // end for (i)
Matrix<double> B_C(2*N, 3*N);
multiply_A_B(B, C, B_C);
Matrix<double> T(2*N, 2*N);
multiply_A_Bt(B_C, B, T);
Matrix<double> Tinv;
invertMatrix(T, Tinv);
Matrix<double> NN(5, 5), N4(4, 4);
Matrix<double> At_Tinv(4, 2*N);
multiply_At_B(A, Tinv, At_Tinv);
multiply_A_B(At_Tinv, A, N4);
for (int r = 0; r < 4; ++r)
for (int c = 0; c < 4; ++c)
NN[r][c] = N4[r][c];
NN[0][4] = NN[4][0] = X[0];
NN[1][4] = NN[4][1] = X[1];
NN[2][4] = NN[4][2] = X[2];
NN[3][4] = NN[4][3] = X[3];
NN[4][4] = 0.0;
Matrix<double> Ninv(5, 5);
invertMatrix(NN, Ninv);
Matrix4x4d sigma_XX;
for (int r = 0; r < 4; ++r)
for (int c = 0; c < 4; ++c)
sigma_XX[r][c] = Ninv[r][c];
Matrix3x4d Je;
makeZeroMatrix(Je);
Je[0][0] = Je[1][1] = Je[2][2] = 1.0 / X[3];
Je[0][3] = -X[0] / (X[3]*X[3]);
Je[1][3] = -X[1] / (X[3]*X[3]);
Je[2][3] = -X[2] / (X[3]*X[3]);
Matrix3x3d sigma_X = Je * sigma_XX * Je.transposed();
return sigma_X;
}
m4x4 m4x4::inverse(){
m4x4 m;
invertMatrix(*this,m);
return m;
}
/*
** This is a screwball function. What it does is the following:
** Given screen x and y coordinates, compute the corresponding object space
** x and y coordinates given that the object space z is 0.9 + OFFSETZ.
** Since the tops of (most) pieces are at z = 0.9 + OFFSETZ, we use that
** number.
*/
int
computeCoords(int piece, int mousex, int mousey,
GLfloat * selx, GLfloat * sely)
{
GLfloat modelMatrix[16];
GLfloat projMatrix[16];
GLfloat finalMatrix[16];
GLfloat in[4];
GLfloat a, b, c, d;
GLfloat top, bot;
GLfloat z;
GLfloat w;
GLfloat height;
if (piece == 0)
return 0;
height = zsize[piece] - 0.1 + OFFSETZ;
glGetFloatv(GL_PROJECTION_MATRIX, projMatrix);
glGetFloatv(GL_MODELVIEW_MATRIX, modelMatrix);
multMatrices(modelMatrix, projMatrix, finalMatrix);
if (!invertMatrix(finalMatrix, finalMatrix))
return 0;
in[0] = (2.0 * (mousex - viewport[0]) / viewport[2]) - 1;
in[1] = (2.0 * ((H - mousey) - viewport[1]) / viewport[3]) - 1;
a = in[0] * finalMatrix[0 * 4 + 2] +
in[1] * finalMatrix[1 * 4 + 2] +
finalMatrix[3 * 4 + 2];
b = finalMatrix[2 * 4 + 2];
c = in[0] * finalMatrix[0 * 4 + 3] +
in[1] * finalMatrix[1 * 4 + 3] +
finalMatrix[3 * 4 + 3];
d = finalMatrix[2 * 4 + 3];
/*
** Ok, now we need to solve for z: ** (a + b z) / (c + d
z) = height. ** ("height" is the height in object space we
want to solve z for) ** ** ==> a + b z = height c +
height d z ** bz - height d z = height c - a ** z =
(height c - a) / (b - height d) */
top = height * c - a;
bot = b - height * d;
if (bot == 0.0)
return 0;
z = top / bot;
/*
** Ok, no problem. ** Now we solve for x and y. We know
that w = c + d z, so we compute it. */
w = c + d * z;
/*
** Now for x and y: */
*selx = (in[0] * finalMatrix[0 * 4 + 0] +
in[1] * finalMatrix[1 * 4 + 0] +
z * finalMatrix[2 * 4 + 0] +
finalMatrix[3 * 4 + 0]) / w - OFFSETX;
*sely = (in[0] * finalMatrix[0 * 4 + 1] +
in[1] * finalMatrix[1 * 4 + 1] +
z * finalMatrix[2 * 4 + 1] +
finalMatrix[3 * 4 + 1]) / w - OFFSETY;
return 1;
}
static void getMatrix()
{
glGetDoublev(GL_MODELVIEW_MATRIX, _matrix);
invertMatrix(_matrix, _matrixInverse);
}
static void display(void)
{
/* World-space positions for light and eye. */
const float eyePosition[4] = { camera[0], camera[1], camera[2], 1 };
const float lightPosition[4] = { 0,100,0, 1 };
float translateMatrix[16], rotateMatrix[16],
modelMatrix[16], invModelMatrix[16], viewMatrix[16],
modelViewMatrix[16], modelViewProjMatrix[16];
float objSpaceEyePosition[4], objSpaceLightPosition[4];
buildLookAtMatrix(eyePosition[0], eyePosition[1], eyePosition[2],
0, 0, 0,
0, 1, 0,
viewMatrix);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
cgGLBindProgram(myCgVertexProgram);
checkForCgError("binding vertex program");
cgGLEnableProfile(myCgVertexProfile);
checkForCgError("enabling vertex profile");
cgGLBindProgram(myCgFragmentProgram);
checkForCgError("binding fragment program");
cgGLEnableProfile(myCgFragmentProfile);
checkForCgError("enabling fragment profile");
/*** Render able ***/
setTableMaterial();
/* modelView = rotateMatrix * translateMatrix */
makeRotateMatrix(0, 1, 1, 1, rotateMatrix);
makeTranslateMatrix(0, 0 , 0, translateMatrix);
multMatrix(modelMatrix, translateMatrix, rotateMatrix);
/* invModelMatrix = inverse(modelMatrix) */
invertMatrix(invModelMatrix, modelMatrix);
/* Transform world-space eye and light positions to sphere's object-space. */
transform(objSpaceEyePosition, invModelMatrix, eyePosition);
cgSetParameter3fv(myCgFragmentParam_eyePosition, objSpaceEyePosition);
transform(objSpaceLightPosition, invModelMatrix, lightPosition);
cgSetParameter3fv(myCgFragmentParam_lightPosition, objSpaceLightPosition);
/* modelViewMatrix = viewMatrix * modelMatrix */
multMatrix(modelViewMatrix, viewMatrix, modelMatrix);
/* modelViewProj = projectionMatrix * modelViewMatrix */
multMatrix(modelViewProjMatrix, myProjectionMatrix, modelViewMatrix);
/* Set matrix parameter with row-major matrix. */
cgSetMatrixParameterfr(myCgVertexParam_modelViewProj, modelViewProjMatrix);
drawtable();
collision();
/*** Render White Ball ***/
setBallMaterial(w.r, w.g, w.b);
if (w.xspeed>0) { w.xspeed=w.xspeed-friction; if (w.xspeed<=0){ w.xspeed=0;}else w.x=w.x+(w.xspeed);}
else if(w.xspeed<0){ w.xspeed=w.xspeed+friction; if (w.xspeed>=0) {w.xspeed=0;}else w.x=w.x+(w.xspeed);}
else { w.xspeed = 0;
}
if (w.zspeed>0) {w.zspeed=w.zspeed-friction; if (w.zspeed<=0) {w.zspeed=0;}else w.z=w.z+(w.zspeed);}
else if (w.zspeed<0){ w.zspeed=w.zspeed+friction; if (w.zspeed>=0) {w.zspeed=0;}else w.z=w.z+(w.zspeed);}
else { w.zspeed=0;
}
if (w.xspeed==0 && w.zspeed==0)
{b.flag=1;
w.flag=1;
r.flag=1;
glutPostRedisplay();
}
board();
/* modelView = viewMatrix * translateMatrix */
makeTranslateMatrix(w.x, w.y, w.z, translateMatrix);
makeRotateMatrix(90, 1, 0, 0, rotateMatrix);
multMatrix(modelMatrix, translateMatrix, rotateMatrix);
/* invModelMatrix = inverse(modelMatrix) */
invertMatrix(invModelMatrix, modelMatrix);
/* Transform world-space eye and light positions to sphere's object-space. */
transform(objSpaceEyePosition, invModelMatrix, eyePosition);
cgSetParameter3fv(myCgFragmentParam_eyePosition, objSpaceEyePosition);
transform(objSpaceLightPosition, invModelMatrix, lightPosition);
cgSetParameter3fv(myCgFragmentParam_lightPosition, objSpaceLightPosition);
/* modelViewMatrix = viewMatrix * modelMatrix */
multMatrix(modelViewMatrix, viewMatrix, modelMatrix);
/* modelViewProj = projectionMatrix * modelViewMatrix */
multMatrix(modelViewProjMatrix, myProjectionMatrix, modelViewMatrix);
/* Set matrix parameter with row-major matrix. */
cgSetMatrixParameterfr(myCgVertexParam_modelViewProj, modelViewProjMatrix);
glutWireSphere(4, 30, 30);
/*** Render Red Ball ***/
setBallMaterial(r.r,r.g,r.b);
collision();
//checkzerospeed();
if (r.xspeed>0){r.xspeed=r.xspeed-friction; if (r.xspeed<=0) { r.xspeed=0;}else r.x=r.x+(r.xspeed);}
else if(r.xspeed<0){r.xspeed=r.xspeed+friction; if (r.xspeed>=0) {r.xspeed=0;}else r.x=r.x+(r.xspeed);}
else { r.xspeed = 0;
}
if (r.zspeed>0){r.zspeed=r.zspeed-friction; if (r.zspeed<=0){ r.zspeed=0;}else r.z=r.z+(r.zspeed);}
else if (r.zspeed<0){r.zspeed=r.zspeed+friction; if (r.zspeed>=0) { r.zspeed=0;}else r.z=r.z+(r.zspeed);}
else r.zspeed=0;
if (r.xspeed==0 && r.zspeed==0)
{b.flag=1;
w.flag=1;
r.flag=1;
glutPostRedisplay();
}
redboard();
/* modelView = viewMatrix * translateMatrix */
makeTranslateMatrix(r.x, r.y, r.z, translateMatrix);
makeRotateMatrix(90, 1, 0, 0, rotateMatrix);
multMatrix(modelMatrix, translateMatrix, rotateMatrix);
/* invModelMatrix = inverse(modelMatrix) */
invertMatrix(invModelMatrix, modelMatrix);
/* Transform world-space eye and light positions to sphere's object-space. */
transform(objSpaceEyePosition, invModelMatrix, eyePosition);
cgSetParameter3fv(myCgFragmentParam_eyePosition, objSpaceEyePosition);
transform(objSpaceLightPosition, invModelMatrix, lightPosition);
cgSetParameter3fv(myCgFragmentParam_lightPosition, objSpaceLightPosition);
/* modelViewMatrix = viewMatrix * modelMatrix */
multMatrix(modelViewMatrix, viewMatrix, modelMatrix);
/* modelViewProj = projectionMatrix * modelViewMatrix */
multMatrix(modelViewProjMatrix, myProjectionMatrix, modelViewMatrix);
/* Set matrix parameter with row-major matrix. */
cgSetMatrixParameterfr(myCgVertexParam_modelViewProj, modelViewProjMatrix);
glutSolidSphere(4, 30, 30);
/*** Render Black Ball ***/
setBallMaterial(b.r,b.g,b.b);
collision();
if (b.xspeed>0){b.xspeed=b.xspeed-friction; if (b.xspeed<=0) {b.xspeed=0;}else b.x=b.x+(b.xspeed);}
else if(b.xspeed<0){b.xspeed=b.xspeed+friction; if (b.xspeed>=0) { b.xspeed=0;}else b.x=b.x+(b.xspeed);}
else { b.xspeed = 0;
}
if (b.zspeed>0){b.zspeed=b.zspeed-friction; if (b.zspeed<=0){ b.zspeed=0;}else b.z=b.z+(b.zspeed);}
else if (b.zspeed<0){b.zspeed=b.zspeed+friction; if (b.zspeed>=0) { b.zspeed=0;}else b.z=b.z+(b.zspeed);}
else b.zspeed=0;
if (b.xspeed==0 && b.zspeed==0)
{b.flag=1;
w.flag=1;
r.flag=1;
glutPostRedisplay();
}
blackboard();
/* modelView = viewMatrix * translateMatrix */
makeTranslateMatrix(b.x, b.y, b.z, translateMatrix);
makeRotateMatrix(90, 1, 0, 0, rotateMatrix);
multMatrix(modelMatrix, translateMatrix, rotateMatrix);
/* invModelMatrix = inverse(modelMatrix) */
invertMatrix(invModelMatrix, modelMatrix);
/* Transform world-space eye and light positions to sphere's object-space. */
transform(objSpaceEyePosition, invModelMatrix, eyePosition);
cgSetParameter3fv(myCgFragmentParam_eyePosition, objSpaceEyePosition);
transform(objSpaceLightPosition, invModelMatrix, lightPosition);
cgSetParameter3fv(myCgFragmentParam_lightPosition, objSpaceLightPosition);
/* modelViewMatrix = viewMatrix * modelMatrix */
multMatrix(modelViewMatrix, viewMatrix, modelMatrix);
/* modelViewProj = projectionMatrix * modelViewMatrix */
multMatrix(modelViewProjMatrix, myProjectionMatrix, modelViewMatrix);
/* Set matrix parameter with row-major matrix. */
cgSetMatrixParameterfr(myCgVertexParam_modelViewProj, modelViewProjMatrix);
glutSolidSphere(4, 30, 30);
/*** Render Stick***/
if (enablestick)
{ //printf("stick enabled");
setStickMaterial();
/* modelView = viewMatrix * translateMatrix */
makeTranslateMatrix(w.x,0,w.z, translateMatrix);
makeRotateMatrix(stickangle, 0, 1, 0, rotateMatrix);
multMatrix(modelMatrix, translateMatrix, rotateMatrix);
/* invModelMatrix = inverse(modelMatrix) */
invertMatrix(invModelMatrix, modelMatrix);
/* Transform world-space eye and light positions to sphere's object-space. */
transform(objSpaceEyePosition, invModelMatrix, eyePosition);
cgSetParameter3fv(myCgFragmentParam_eyePosition, objSpaceEyePosition);
transform(objSpaceLightPosition, invModelMatrix, lightPosition);
cgSetParameter3fv(myCgFragmentParam_lightPosition, objSpaceLightPosition);
/* modelViewMatrix = viewMatrix * modelMatrix */
multMatrix(modelViewMatrix, viewMatrix, modelMatrix);
/* modelViewProj = projectionMatrix * modelViewMatrix */
multMatrix(modelViewProjMatrix, myProjectionMatrix, modelViewMatrix);
/* Set matrix parameter with row-major matrix. */
cgSetMatrixParameterfr(myCgVertexParam_modelViewProj, modelViewProjMatrix);
drawstick();
/* glBegin(GL_LINES);
glVertex3f(0,0,0);
glVertex3f(0,20,84);
glEnd();
*/
}
glutSwapBuffers();
}
void CGlObjLoader::getMatrix()
{
glGetDoublev(GL_MODELVIEW_MATRIX, _matrix);
invertMatrix(_matrix, _matrixI);
}
//
// The guts of the shadow determination algorithm.
//
void Renderer::determineShadows (vector<Caster> casters, const Light& light,
Camera& camera)
{
glPushAttrib(GL_ALL_ATTRIB_BITS);
glDepthMask(0); // Disable depth buffer changes.
glDisable(GL_CULL_FACE); // Enable drawing of backfaces.
glEnable(GL_STENCIL_TEST); // Enable the stencil buffer.
glStencilFunc(GL_ALWAYS, 0, ~0); // Always pass the stencil test.
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LESS);
// If we draw shadow volumes then we need to specify a color, otherwise
// we disable drawing into the frame buffer.
if (global.drawShadowVolumes)
{
glColor4f(1.0, 0.0, 0.0, 0.2);
}
else
{
glColorMask(0, 0, 0, 0);
}
for (vector<Caster>::iterator caster = casters.begin();
caster != casters.end(); ++caster)
{
if (!caster->isCaster())
continue;
// Get the matricies required to draw the caster, and load a matrix which
// places the caster into Camera space.
glPushMatrix();
const Matrix& localToWorld = caster->getLocalToWorldMatrix();
glMultMatrix(localToWorld);
// Transform the light position into object (or local) space by inverting
// the localToWorld matrix and transforming the light position.
Vec3 lightPosLocal = light.getPosition();
invertMatrix(localToWorld).transform(lightPosLocal);
caster->getModel()->bindExtrudeBuffer();
// Setup the shader program for use.
extrudeShader->useProgram();
glUniform4f(glGetUniformLocation(extrudeShader->getId(), "lightPos"),
lightPosLocal.x, lightPosLocal.y, lightPosLocal.z, lightPosLocal.w);
// TODO: Add z-Fail testing here.
// z-Pass algorithm.
//setStencilOp(GL_KEEP, GL_INCR_WRAP, GL_KEEP, GL_DECR_WRAP);
//drawVolumeSides(lightPosLocal, *caster);
// z-fail method (Carmacks Reverse). Works for nearly all situations but
// isn't as efficient as the z-pass method above as it draws both the
// front and back caps whereas the other methods doesn't require this.
if (1)
{
setStencilOp(GL_DECR_WRAP, GL_KEEP, GL_INCR_WRAP, GL_KEEP);
drawVolumeSides(lightPosLocal, *caster);
drawDarkCap(lightPosLocal, *caster);
drawLightCap(lightPosLocal, *caster);
}
/*
// This is an alternative method for drawing the shadow volumes which
// works but takes two passes and isn't as efficient. This is the
// z-pass method too.
glEnable(GL_CULL_FACE);
glCullFace(GL_BACK);
glStencilOp(GL_KEEP, GL_KEEP, GL_INCR);
drawVolumeSides(lightPosLocal, *caster);
glCullFace(GL_FRONT);
glStencilOp(GL_KEEP, GL_KEEP, GL_DECR);
drawVolumeSides(lightPosLocal, *caster);
*/
extrudeShader->disableProgram();
glPopMatrix();
}
glPopAttrib();
}
