/*! The matrix, when multiplied with a wrench applied at this contact will give the
resultant wrench applied on the other body in contact (thus computed relative
to that object's center of mass), expressed in world coordinates.
The matrix looks like this:
| R 0 |
|CR R |
Where R is the 3x3 rotation matrix between the coordinate systems and C also
contains the cross product matrix that depends on the translation between them.
*/
Matrix
Contact::localToWorldWrenchMatrix() const
{
Matrix Ro(Matrix::ZEROES<Matrix>(6,6));
transf contactTran = getContactFrame() * getBody1()->getTran();
mat3 R; contactTran.rotation().ToRotationMatrix(R);
Matrix Rot( Matrix::ROTATION(R) );
//the force transform is simple, just the matrix that changes coord. systems
Ro.copySubMatrix(0, 0, Rot);
Ro.copySubMatrix(3, 3, Rot);
//for torque we also multiply by a cross product matrix
vec3 worldLocation = contactTran.translation();
vec3 cog = getBody2()->getTran().translation();
mat3 C; C.setCrossProductMatrix(worldLocation - cog);
Matrix CR(3,3);
matrixMultiply(Matrix::ROTATION(C.transpose()), Rot, CR);
//also scale by object max radius so we get same units as force
//and optimization does not favor torque over force
double scale = 1.0;
if (getBody2()->isA("GraspableBody")) {
scale = scale / static_cast<GraspableBody*>(getBody2())->getMaxRadius();
}
CR.multiply(scale);
Ro.copySubMatrix(3, 0, CR);
return Ro;
}
/*! Assembles together the localToWorldWrenchMatrix for all the contacts in the list
in block diagonal form.
*/
Matrix
Contact::localToWorldWrenchBlockMatrix(const std::list<Contact*> &contacts)
{
if (contacts.empty()) {
return Matrix(0,0);
}
std::list<Matrix*> blocks;
std::list<Contact*>::const_iterator it;
for (it=contacts.begin(); it!=contacts.end(); it++) {
blocks.push_back( new Matrix((*it)->localToWorldWrenchMatrix()) );
}
Matrix Ro(Matrix::BLOCKDIAG<Matrix>(&blocks));
while (!blocks.empty()) {
delete blocks.back();
blocks.pop_back();
}
return Ro;
}
int main() {
double X, Y, Z;
double max, N, t1, t2;
double owx, owy, owz, c, e;
double Fi, Fj, Fk, F1;
int i, j, k, mi, mj, mk;
int R, fl, fl1, fl2;
int it,f, L0, L1;
long int osdt;
struct timeval tv1, tv2;
it = 0;
X = 2.0;
Y = 2.0;
Z = 2.0;
e = 0.00001;
L0 = 1;
L1 = 0;
/* Размеры шагов */
hx = X / in;
hy = Y / jn;
hz = Z / kn;
owx = pow(hx,2);
owy = pow(hy,2);
owz = pow(hz,2);
c = 2 / owx + 2 / owy + 2 / owz + a;
gettimeofday(&tv1, NULL);
/* Инициализация границ пространства */
Inic();
/* Основной итерационный цикл */
do {
f = 1;
L0 = 1 - L0;
L1 = 1 - L1;
for (i = 1; i < in; i++) {
for (j = 1; j < jn; j++) {
for(k = 1; k < kn; k++) {
Fi = (F[L0][i+1][j][k] + F[L0][i-1][j][k]) / owx;
Fj = (F[L0][i][j+1][k] + F[L0][i][j-1][k]) / owy;
Fk = (F[L0][i][j][k+1] + F[L0][i][j][k-1]) / owz;
F[L1][i][j][k] = (Fi + Fj + Fk - Ro(i*hx,j*hy,k*hz)) / c;
if (fabs(F[L1][i][j][k] - F[L0][i][j][k]) > e)
f = 0;
}
}
}
it++;
} while (!f);
gettimeofday(&tv2, NULL);
osdt = (tv2.tv_sec - tv1.tv_sec)*1000000 + tv2.tv_usec - tv1.tv_usec;
printf("in = %d ; iter = %d ; E = %f ; T = %ld\n", in, it, e, osdt);
/* Нахождение максимального расхождения полученного приближенного решения
* и точного решения */
max = 0.0;
for(i = 1; i < in; i++) {
for(j = 1; j < jn; j++) {
for(k = 1; k < kn; k++) {
if ( (F1 = fabs(F[L1][i][j][k] - Fresh(i*hx,j*hy,k*hz))) > max ) {
max = F1;
mi = i;
mj = j;
mk = k;
}
}
}
}
printf("Max differ = %f\n in point(%d,%d,%d)\n", max, mi, mj, mk);
return 0;
}
