/*! Given a vector of pointers to dynamic bodies, and the number of bodies in
the vector, this routine will move those bodies in the direction of their
current velocity for the length of the timestep, \a h. It uses the
pre-computed velocities and accelerations computed by iterateDynamics and
stored for each body.
*/
int
moveBodies(int numBodies,std::vector<DynamicBody *> bodyVec,double h)
{
static double V[42];
static double tmp12[12];
static double B[12];
static double R_N_B[9];
static double newPos[7];
static mat3 Rot;
int bn;
double currq[7];
double currv[6];
int errCode=SUCCESS;
for (bn=0;bn<numBodies;bn++) {
memcpy(currq,bodyVec[bn]->getPos(),7*sizeof(double));
memcpy(currv,bodyVec[bn]->getVelocity(),6*sizeof(double));;
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];
// B relates the angular velocity of the body (expressed in
// the body frame) to the time derivative of the Euler parameters
B[0] = -currq[4]; B[4] = -currq[5]; B[8] = -currq[6];
B[1] = currq[3]; B[5] = -currq[6]; B[9] = currq[5];
B[2] = currq[6]; B[6] = currq[3]; B[10]= -currq[4];
B[3] = -currq[5]; B[7] = currq[4]; B[11]= currq[3];
dscal(12,0.5,B,1);
// V is a list of matrices. Each matrix (V_bn) can be multiplied by
// body bn's 6x1 velocity vector to get the 7x1 time derivative
// of the body's position.
// V_bn = [ eye(3,3) zeros(3,3);
// zeros(4,3) B*R_N_B' ];
// This list of matrices will be used at the end to compute the new
// position from the new velocity
dgemm("N","T",4,3,3,1.0,B,4,R_N_B,3,0.0,tmp12,4);
V[0] = 1.0;
V[8] = 1.0;
V[16]= 1.0;
fillMatrixBlock(tmp12,4,3,3,6,5,V,7);
dcopy(7,currq,1,newPos,1);
dgemv("N",7,6,h,V,7,currv,1,1.0,newPos,1);
#ifdef GRASPITDBG
fprintf(stdout,"object %s new velocity: \n", bodyVec[bn]->getName().latin1());
for (int i=0;i<6;i++) fprintf(stdout,"%le ",currv[i]);
printf("\n");
fprintf(stdout,"object %s new position: \n", bodyVec[bn]->getName().latin1());
disp_mat(stdout,newPos,1,7,0);
#endif
//should we bother to check if the object has moved? (for optimization)
if (!bodyVec[bn]->setPos(newPos)) {
DBGP("requested position is out of bounds for this object!");
errCode = FAILURE;
}
}
return errCode;
}
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]); );
int main(int argc, char** argv)
{
CvMat* M = cvCreateMat(3, 3, CV_64FC1);
cvSetIdentity(M);
CvMat* L = MT_CreateCholeskyResult(M);
MT_Cholesky(M, L);
disp_mat(M, "M");
disp_mat(L, "L = chol(M)");
cvmSet(M, 1, 1, 4.0);
cvmSet(M, 2, 2, 9.0);
MT_Cholesky(M, L);
disp_mat(M, "M");
disp_mat(L, "L = chol(M)");
cvmSet(M, 0, 1, 0.1);
cvmSet(M, 0, 2, 0.1);
cvmSet(M, 1, 0, 0.1);
cvmSet(M, 1, 2, 0.1);
cvmSet(M, 2, 0, 0.1);
cvmSet(M, 2, 1, 0.1);
MT_Cholesky(M, L);
disp_mat(M, "M");
disp_mat(L, "L = chol(M)");
cvReleaseMat(&M);
cvReleaseMat(&L);
M = cvCreateMat(5, 5, CV_64FC1);
cvSet(M, cvScalar(0.1));
for(unsigned int i = 0; i < 5; i++)
{
cvmSet(M, i, i, (double) (i+1));
}
L = MT_CreateCholeskyResult(M);
MT_Cholesky(M, L);
disp_mat(M, "M");
disp_mat(L, "L = chol(M)");
cvReleaseMat(&M);
cvReleaseMat(&L);
CvMat* C = cvCreateMat(4, 4, CV_64FC1);
CvMat* A = cvCreateMatHeader(2, 2, CV_64FC1);
CvMat* B = cvCreateMatHeader(2, 2, CV_64FC1);
cvGetSubRect(C, A, cvRect(0, 0, 2, 2));
cvGetSubRect(C, B, cvRect(2, 2, 2, 2));
cvSet(C, cvScalar(0));
cvSet(A, cvScalar(1));
cvSet(B, cvScalar(2));
cvReleaseMat(&A);
cvReleaseMat(&B);
disp_mat(C, "C (composited)");
cvReleaseMat(&C);
}
