Program: 70860
Project: 70860
Buggy source:
/** 
 * Solves a KKT system in which the Jacobian augmented M matrix and and force vectors are given by <pre> M' = M + a0 df/dv + a1 df/dx  bf' = bf + (a2 df/dv + a3 df/dx) vel0 </pre> It is assumed that a0 and a1 are both non-zero. It is also assumed that the a0 = -alpha h, where h is the step size and alpha indicates the propertion of implicitness for the solve; i.e., for regular backward euler, alpha=1, while for trapezoidal solves, alpha = 0.5; When used to solve for velocities in an implicit integrator, then on input, bf is assumed to be given by <pre> bf = M vel0 + h f </pre> where h is the time step and f is the generalized forces, while on output bf is modified to include the Jacobian terms described above.
 * @param vel returns the computed velocity
 * @param fpar if useFictitousJacobianForces is true, returns fictitious Jacobian forces for parametric components
 * @param bf right side offset
 * @param btmp temporary vector
 * @param vel0 right side velocity
 * @param h interval time step - used to scale constraint offsets and impulses
 * @param a0 left side df/dv coefficient
 * @param a1 left side df/dx coefficient
 * @param a2 right side df/dv coefficient
 * @param a3 right side df/dx coefficient
 */
public void KKTFactorAndSolve(VectorNd vel,VectorNd fpar,VectorNd bf,VectorNd btmp,VectorNd vel0,double h,double a0,double a1,double a2,double a3){
  updateStateSizes();
  int velSize=myActiveVelSize;
  boolean analyze=false;
  updateSolveMatrixStructure();
  if (myKKTSolveMatrixVersion != mySolveMatrixVersion) {
    myKKTSolveMatrixVersion=mySolveMatrixVersion;
    analyze=true;
  }
  SparseNumberedBlockMatrix S=mySolveMatrix;
  S.setZero();
  myC.setSize(S.rowSize());
  myC.setZero();
  mySys.addVelJacobian(S,myC,a0);
  if (useFictitousJacobianForces) {
    bf.scaledAdd(-a0,myC);
    if (fpar != null && myParametricVelSize > 0) {
      setSubVector(fpar,myC,velSize,myParametricVelSize);
    }
  }
  if (vel0 != null) {
    double alpha=a2 / a0 - a3 / a1;
    S.mul(btmp,vel0,velSize,velSize);
    bf.scaledAdd(alpha,btmp);
  }
  myC.setZero();
  mySys.addPosJacobian(S,myC,a1);
  if (useFictitousJacobianForces) {
    bf.scaledAdd(-a0,myC);
    if (fpar != null && myParametricVelSize > 0) {
      addSubVector(fpar,myC,velSize,myParametricVelSize);
    }
  }
  if (vel0 != null && a3 != 0) {
    double beta=a3 / a1;
    S.mul(btmp,vel0,velSize,velSize);
    bf.scaledAdd(beta,btmp);
  }
  addActiveMassMatrix(mySys,S);
  if (velSize > 0 && myParametricVelSize > 0) {
    S.mulTranspose(btmp,myUpar,0,velSize,velSize,myParametricVelSize);
    bf.sub(btmp);
  }
  if (myKKTSolver == null) {
    myKKTSolver=new KKTSolver();
  }
  updateBilateralConstraints();
  if (myKKTGTVersion != myGTVersion) {
    analyze=true;
    myKKTGTVersion=myGTVersion;
  }
  if (myGsize > 0 && myParametricVelSize > 0) {
    myGT.mulTranspose(myBg,myUpar,0,myGsize,velSize,myParametricVelSize);
    myBg.negate();
  }
 else {
    myBg.setZero();
  }
  setBilateralOffsets(h,-a0);
  updateUnilateralConstraints();
  if (myNsize > 0 && myParametricVelSize > 0) {
    myNT.mulTranspose(myBn,myUpar,0,myNsize,velSize,myParametricVelSize);
    myBn.negate();
  }
 else {
    myBn.setZero();
  }
  setUnilateralOffsets(h,-a0);
  mySys.getBilateralImpulses(myLam);
  mySys.getUnilateralImpulses(myThe);
  if (!solveModePrinted) {
    String msg=(myHybridSolveP ? "String_Node_Str" : "String_Node_Str");
    if (mySys.getSolveMatrixType() == Matrix.INDEFINITE) {
      msg+="String_Node_Str";
    }
 else {
      msg+="String_Node_Str";
    }
    System.out.println(msg);
    solveModePrinted=true;
  }
  if (crsWriter == null && crsFileName != null) {
    try {
      crsWriter=ArtisynthIO.newIndentingPrintWriter(crsFileName);
    }
 catch (    Exception e) {
      crsFileName=null;
    }
  }
  if (velSize != 0) {
    if (vel0 != null) {
      vel.set(vel0);
    }
    if (analyze) {
      myKKTSolver.analyze(S,velSize,myGT,mySys.getSolveMatrixType());
    }
    if (myHybridSolveP && !analyze && myNT.colSize() == 0) {
      if (profileKKTSolveTime) {
        timerStart();
      }
      myKKTSolver.factorAndSolve(S,velSize,myGT,myRg,vel,myLam,bf,myBg,myHybridSolveTol);
      if (profileKKTSolveTime) {
        timerStop("String_Node_Str");
      }
    }
 else {
      if (profileKKTSolveTime) {
        timerStart();
      }
      myKKTSolver.factor(S,velSize,myGT,myRg,myNT,myRn);
      myKKTSolver.solve(vel,myLam,myThe,bf,myBg,myBn);
      if (profileKKTSolveTime) {
        timerStop("String_Node_Str");
      }
    }
    if (computeKKTResidual) {
      double res=myKKTSolver.residual(S,velSize,myGT,myRg,myNT,myRn,vel,myLam,myThe,bf,myBg,myBn);
      System.out.println("String_Node_Str" + velSize + "String_Node_Str"+ myGT.colSize()+ "String_Node_Str"+ myNT.colSize()+ "String_Node_Str"+ res);
    }
    if (crsWriter != null) {
      String msg="String_Node_Str" + velSize + "String_Node_Str"+ myGT.colSize()+ "String_Node_Str"+ myNT.colSize()+ (analyze ? "String_Node_Str" : "String_Node_Str");
      System.out.println(msg);
      try {
        crsWriter.println(msg);
        myKKTSolver.printLinearProblem(crsWriter,bf,myBg,"String_Node_Str",crsOmitDiag);
      }
 catch (      Exception e) {
        e.printStackTrace();
        crsWriter=null;
        crsFileName=null;
      }
    }
  }
  mySys.setBilateralImpulses(myLam,h);
  mySys.setUnilateralImpulses(myThe,h);
  if (myUpdateForcesAtStepEnd) {
    if (myGsize > 0) {
      myGT.mulAdd(myFcon,myLam,velSize,myGsize);
    }
    if (myNsize > 0) {
      myNT.mulAdd(myFcon,myThe,velSize,myNsize);
    }
  }
  if (myLogWriter != null) {
    try {
      NumberFormat fmt=new NumberFormat("String_Node_Str");
      myLogWriter.println("String_Node_Str" + velSize + "String_Node_Str"+ velSize+ "String_Node_Str");
      S.write(myLogWriter,fmt,Matrix.WriteFormat.SYMMETRIC_CRS,velSize,velSize);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str" + velSize + "String_Node_Str"+ myGT.colSize()+ "String_Node_Str");
      myGT.write(myLogWriter,fmt,Matrix.WriteFormat.CRS,velSize,myGT.colSize());
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str" + velSize + "String_Node_Str"+ myNT.colSize()+ "String_Node_Str");
      myNT.write(myLogWriter,fmt,Matrix.WriteFormat.CRS,velSize,myNT.colSize());
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      bf.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      myBg.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      myBn.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      vel.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      myLam.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      myThe.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      myLogWriter.flush();
      System.out.println("String_Node_Str");
    }
 catch (    IOException e) {
      e.printStackTrace();
      myLogWriter=null;
    }
  }
}

Fixed version (ground truth):
/** 
 * Solves a KKT system in which the Jacobian augmented M matrix and and force vectors are given by <pre> M' = M + a0 df/dv + a1 df/dx  bf' = bf + (a2 df/dv + a3 df/dx) vel0 </pre> It is assumed that a0 and a1 are both non-zero. It is also assumed that the a0 = -alpha h, where h is the step size and alpha indicates the propertion of implicitness for the solve; i.e., for regular backward euler, alpha=1, while for trapezoidal solves, alpha = 0.5; When used to solve for velocities in an implicit integrator, then on input, bf is assumed to be given by <pre> bf = M vel0 + h f </pre> where h is the time step and f is the generalized forces, while on output bf is modified to include the Jacobian terms described above.
 * @param vel returns the computed velocity
 * @param fpar if useFictitousJacobianForces is true, returns fictitious Jacobian forces for parametric components
 * @param bf right side offset
 * @param btmp temporary vector
 * @param vel0 right side velocity
 * @param h interval time step - used to scale constraint offsets and impulses
 * @param a0 left side df/dv coefficient
 * @param a1 left side df/dx coefficient
 * @param a2 right side df/dv coefficient
 * @param a3 right side df/dx coefficient
 */
public void KKTFactorAndSolve(VectorNd vel,VectorNd fpar,VectorNd bf,VectorNd btmp,VectorNd vel0,double h,double a0,double a1,double a2,double a3){
  updateStateSizes();
  int velSize=myActiveVelSize;
  boolean analyze=false;
  updateSolveMatrixStructure();
  if (myKKTSolveMatrixVersion != mySolveMatrixVersion) {
    myKKTSolveMatrixVersion=mySolveMatrixVersion;
    analyze=true;
  }
  SparseNumberedBlockMatrix S=mySolveMatrix;
  S.setZero();
  myC.setSize(S.rowSize());
  myC.setZero();
  mySys.addVelJacobian(S,myC,a0);
  if (useFictitousJacobianForces) {
    bf.scaledAdd(-a0,myC);
    if (fpar != null && myParametricVelSize > 0) {
      setSubVector(fpar,myC,velSize,myParametricVelSize);
    }
  }
  if (vel0 != null) {
    double alpha=a2 / a0 - a3 / a1;
    S.mul(btmp,vel0,velSize,velSize);
    bf.scaledAdd(alpha,btmp);
  }
  myC.setZero();
  mySys.addPosJacobian(S,myC,a1);
  if (useFictitousJacobianForces) {
    bf.scaledAdd(-a0,myC);
    if (fpar != null && myParametricVelSize > 0) {
      addSubVector(fpar,myC,velSize,myParametricVelSize);
    }
  }
  if (vel0 != null && a3 != 0) {
    double beta=a3 / a1;
    S.mul(btmp,vel0,velSize,velSize);
    bf.scaledAdd(beta,btmp);
  }
  addActiveMassMatrix(mySys,S);
  if (velSize > 0 && myParametricVelSize > 0) {
    S.mulTranspose(btmp,myUpar,0,velSize,velSize,myParametricVelSize);
    bf.sub(btmp);
  }
  if (myKKTSolver == null) {
    myKKTSolver=new KKTSolver();
  }
  updateBilateralConstraints();
  if (myKKTGTVersion != myGTVersion) {
    analyze=true;
    myKKTGTVersion=myGTVersion;
  }
  if (myGsize > 0 && myParametricVelSize > 0) {
    myGT.mulTranspose(myBg,myUpar,0,myGsize,velSize,myParametricVelSize);
    myBg.negate();
  }
 else {
    myBg.setZero();
  }
  setBilateralOffsets(h,-a0);
  updateUnilateralConstraints();
  if (myNsize > 0 && myParametricVelSize > 0) {
    myNT.mulTranspose(myBn,myUpar,0,myNsize,velSize,myParametricVelSize);
    myBn.negate();
  }
 else {
    myBn.setZero();
  }
  setUnilateralOffsets(h,-a0);
  mySys.getBilateralImpulses(myLam);
  mySys.getUnilateralImpulses(myThe);
  if (!solveModePrinted) {
    String msg=(myHybridSolveP ? "String_Node_Str" : "String_Node_Str");
    if (mySys.getSolveMatrixType() == Matrix.INDEFINITE) {
      msg+="String_Node_Str";
    }
 else {
      msg+="String_Node_Str";
    }
    System.out.println(msg);
    solveModePrinted=true;
  }
  if (crsWriter == null && crsFileName != null) {
    try {
      crsWriter=ArtisynthIO.newIndentingPrintWriter(crsFileName);
    }
 catch (    Exception e) {
      crsFileName=null;
    }
  }
  if (velSize != 0) {
    if (vel0 != null) {
      vel.set(vel0);
    }
    if (analyze) {
      myKKTSolver.analyze(S,velSize,myGT,myRg,mySys.getSolveMatrixType());
    }
    if (myHybridSolveP && !analyze && myNT.colSize() == 0) {
      if (profileKKTSolveTime) {
        timerStart();
      }
      myKKTSolver.factorAndSolve(S,velSize,myGT,myRg,vel,myLam,bf,myBg,myHybridSolveTol);
      if (profileKKTSolveTime) {
        timerStop("String_Node_Str");
      }
    }
 else {
      if (profileKKTSolveTime) {
        timerStart();
      }
      myKKTSolver.factor(S,velSize,myGT,myRg,myNT,myRn);
      myKKTSolver.solve(vel,myLam,myThe,bf,myBg,myBn);
      if (profileKKTSolveTime) {
        timerStop("String_Node_Str");
      }
    }
    if (computeKKTResidual) {
      double res=myKKTSolver.residual(S,velSize,myGT,myRg,myNT,myRn,vel,myLam,myThe,bf,myBg,myBn);
      System.out.println("String_Node_Str" + velSize + "String_Node_Str"+ myGT.colSize()+ "String_Node_Str"+ myNT.colSize()+ "String_Node_Str"+ res);
    }
    if (crsWriter != null) {
      String msg="String_Node_Str" + velSize + "String_Node_Str"+ myGT.colSize()+ "String_Node_Str"+ myNT.colSize()+ (analyze ? "String_Node_Str" : "String_Node_Str");
      System.out.println(msg);
      try {
        crsWriter.println(msg);
        myKKTSolver.printLinearProblem(crsWriter,bf,myBg,"String_Node_Str",crsOmitDiag);
      }
 catch (      Exception e) {
        e.printStackTrace();
        crsWriter=null;
        crsFileName=null;
      }
    }
  }
  mySys.setBilateralImpulses(myLam,h);
  mySys.setUnilateralImpulses(myThe,h);
  if (myUpdateForcesAtStepEnd) {
    if (myGsize > 0) {
      myGT.mulAdd(myFcon,myLam,velSize,myGsize);
    }
    if (myNsize > 0) {
      myNT.mulAdd(myFcon,myThe,velSize,myNsize);
    }
  }
  if (myLogWriter != null) {
    try {
      NumberFormat fmt=new NumberFormat("String_Node_Str");
      myLogWriter.println("String_Node_Str" + velSize + "String_Node_Str"+ velSize+ "String_Node_Str");
      S.write(myLogWriter,fmt,Matrix.WriteFormat.SYMMETRIC_CRS,velSize,velSize);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str" + velSize + "String_Node_Str"+ myGT.colSize()+ "String_Node_Str");
      myGT.write(myLogWriter,fmt,Matrix.WriteFormat.CRS,velSize,myGT.colSize());
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str" + velSize + "String_Node_Str"+ myNT.colSize()+ "String_Node_Str");
      myNT.write(myLogWriter,fmt,Matrix.WriteFormat.CRS,velSize,myNT.colSize());
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      bf.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      myBg.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      myBn.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      vel.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      myLam.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      myThe.write(myLogWriter,fmt);
      myLogWriter.println("String_Node_Str");
      myLogWriter.println("String_Node_Str");
      myLogWriter.flush();
      System.out.println("String_Node_Str");
    }
 catch (    IOException e) {
      e.printStackTrace();
      myLogWriter=null;
    }
  }
}