Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Changeset 5449 in orxonox.OLD for trunk/src/lib/collision_detection


Ignore:
Timestamp:
Oct 29, 2005, 2:38:25 PM (19 years ago)
Author:
bensch
Message:

orxonox/trunk: Jacobi-reloaded

Location:
trunk/src/lib/collision_detection
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/src/lib/collision_detection/lin_alg.h

    r5428 r5449  
    3232  Z = (float *) calloc(100, 32);
    3333
    34   for(ip=1; ip<=N; ip++) {  //initialize V to identity matrix
    35     for (iq=1; iq<=N; iq++)  V[ip][iq]=0;
     34  for(ip=0; ip<3; ip++) {  //initialize V to identity matrix
     35    for (iq=0; iq<3; iq++)  V[ip][iq]=0;
    3636    V[ip][ip]=1;
    3737  }
     
    4444  for (i=1; i<=50; i++) {
    4545    sm=0;
    46     for (ip=1; ip<N; ip++)    //sum off-diagonal elements
    47       for (iq=ip+1; iq<=N; iq++)
     46    for (ip=0; ip<2; ip++)    //sum off-diagonal elements
     47      for (iq=ip+1; iq<3; iq++)
    4848        sm=sm+fabs(A[ip][iq]);
    4949    if (sm==0)
     
    5353      return;       //normal return
    5454    }
    55     if (i < 4)
    5655      tresh=0.2*sm*sm;
    57     else
    58       tresh=0;
    59     for (ip=1; ip<N; ip++) {
    60       for (iq=ip+1; iq<=N; iq++) {
     56    for (ip=0; ip<2; ip++) {
     57      for (iq=ip+1; iq<3; iq++) {
    6158        g=100*fabs(A[ip][iq]);
    6259// after 4 sweeps, skip the rotation if the off-diagonal element is small
     
    8178          D[iq] += h;
    8279          A[ip][iq]=0;
    83           for (j=1; j<ip; j++) {
     80          for (j=0; j<ip; j++) {
    8481            g=A[j][ip];
    8582            h=A[j][iq];
     
    9996            A[iq][j] = h+s*(g-h*tau);
    10097          }
    101           for (j=1; j<=N; j++) {
     98          for (j=0; j<3; j++) {
    10299            g=V[j][ip];
    103100            h=V[j][iq];
     
    109106      } // main iq loop
    110107    } // main ip loop
    111     for (ip=1; ip<=N; ip++) {
     108    for (ip=0; ip<3; ip++) {
    112109      B[ip] += Z[ip];
    113110      D[ip]=B[ip];
  • trunk/src/lib/collision_detection/obb_tree_node.cc

    r5431 r5449  
    360360  Vector*              axis = new Vector[3];                //!< the references to the obb axis
    361361
    362   OBBTreeNode::coMat[1][1] = box->covarianceMatrix[0][0]; OBBTreeNode::coMat[1][2] = box->covarianceMatrix[0][1]; OBBTreeNode::coMat[1][3] = box->covarianceMatrix[0][2];
    363   OBBTreeNode::coMat[2][1] = box->covarianceMatrix[1][0]; OBBTreeNode::coMat[2][2] = box->covarianceMatrix[1][1]; OBBTreeNode::coMat[2][3] = box->covarianceMatrix[1][2];
    364   OBBTreeNode::coMat[3][1] = box->covarianceMatrix[2][0]; OBBTreeNode::coMat[3][2] = box->covarianceMatrix[2][1]; OBBTreeNode::coMat[3][3] = box->covarianceMatrix[2][2];
     362  OBBTreeNode::coMat[0][0] = box->covarianceMatrix[0][0];
     363  OBBTreeNode::coMat[0][1] = box->covarianceMatrix[0][1];
     364  OBBTreeNode::coMat[0][2] = box->covarianceMatrix[0][2];
     365
     366  OBBTreeNode::coMat[1][0] = box->covarianceMatrix[1][0];
     367  OBBTreeNode::coMat[1][1] = box->covarianceMatrix[1][1];
     368  OBBTreeNode::coMat[1][2] = box->covarianceMatrix[1][2];
     369
     370  OBBTreeNode::coMat[2][0] = box->covarianceMatrix[2][0];
     371  OBBTreeNode::coMat[2][1] = box->covarianceMatrix[2][1];
     372  OBBTreeNode::coMat[2][2] = box->covarianceMatrix[2][2];
    365373
    366374//   OBBTreeNode::coMat[0][0] = box->covarianceMatrix[0][0];
     
    393401//   }
    394402
    395   axis[0].x = OBBTreeNode::eigvMat[1][1]; axis[0].y = OBBTreeNode::eigvMat[2][1]; axis[0].z = OBBTreeNode::eigvMat[3][1];
    396   axis[1].x = OBBTreeNode::eigvMat[1][2]; axis[1].y = OBBTreeNode::eigvMat[2][2]; axis[1].z = OBBTreeNode::eigvMat[3][2];
    397   axis[2].x = OBBTreeNode::eigvMat[1][3]; axis[2].y = OBBTreeNode::eigvMat[2][3]; axis[2].z = OBBTreeNode::eigvMat[3][3];
     403  axis[0].x = OBBTreeNode::eigvMat[0][0]; axis[0].y = OBBTreeNode::eigvMat[1][0]; axis[0].z = OBBTreeNode::eigvMat[2][0];
     404  axis[1].x = OBBTreeNode::eigvMat[0][1]; axis[1].y = OBBTreeNode::eigvMat[1][1]; axis[1].z = OBBTreeNode::eigvMat[2][1];
     405  axis[2].x = OBBTreeNode::eigvMat[0][2]; axis[2].y = OBBTreeNode::eigvMat[1][2]; axis[2].z = OBBTreeNode::eigvMat[2][2];
    398406  axis[0].normalize();
    399407  axis[1].normalize();
Note: See TracChangeset for help on using the changeset viewer.