Matrix<T> *MatrixOps<T>::MultM (const Matrix<T> *const matrix1, const Matrix<T> *const matrix2) {
Matrix<T> *resmat = new Matrix<T> (matrix1->rows, matrix2->cols);
MultM (matrix1, matrix2, resmat);
return resmat;
}
void ComputeCurve(int joint)
{
float prod[3][4], tm[4], pos[3];
float t = 0, tinc = (float)CYCLE_STEP/OVERSAMPLE;
int ctlpoint, i;
float BBasis[4][4] = {{-1, 3, -3, 1}, {3, -6, 3, 0},
{-3, 3, 0, 0}, {1, 0, 0, 0}};
int lastindex, newindex;
float pointset[3][4];
for (i = 0; i < 4; i++) /* z's are always zero, only 2-d */
pointset[2][i] = 0;
lastindex = -1;
for(ctlpoint = 0; ctlpoint < RotCurve[joint].numpoints; ctlpoint += 3) {
t = 0;
for (i = 0; i < 4; i++)
pointset[0][i] = RotCurve[joint].xcoords[ctlpoint + i];
for (i = 0; i < 4; i++)
pointset[1][i] = RotCurve[joint].angles[ctlpoint + i];
MultM(pointset, BBasis, prod);
while (t <= 1) {
tm[0] = t*t*t;
tm[1] = t*t;
tm[2] = t;
tm[3] = 1;
MultMV(prod, tm, pos);
newindex = (int)(pos[0]*(CYCLE_SIZE-1));
if ((int)(newindex > lastindex)) { /* go at least one */
Walk_cycle[0][joint][newindex] = pos[1];
lastindex++;
}
t += tinc;
}
}
for (i = 0; i < CYCLE_SIZE; i++) { /* copy to other leg, out-o-phase */
if (MirrorLegs)
Walk_cycle[1][joint][i] =
Walk_cycle[0][joint][i];
else
Walk_cycle[1][joint][i] =
Walk_cycle[0][joint][(i+(CYCLE_SIZE/2))%CYCLE_SIZE];
}
}
