void SphereNEDSSphereNEDSR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
double q_0 = q0(0);
double q_1 = q0(1);
double q_2 = q0(2);
double q_7 = q0(7);
double q_8 = q0(8);
double q_9 = q0(9);
y.setValue(0, distance(q_0, q_1, q_2, r1, q_7, q_8, q_9, r2));
//Approximation _Pc1=_Pc2
_Pc1->setValue(0, (r1 * q_0 + r2 * q_7) / (r1 + r2));
_Pc1->setValue(1, (r1 * q_1 + r2 * q_8) / (r1 + r2));
_Pc1->setValue(2, (r1 * q_2 + r2 * q_9) / (r1 + r2));
_Pc2->setValue(0, (r1 * q_0 + r2 * q_7) / (r1 + r2));
_Pc2->setValue(1, (r1 * q_1 + r2 * q_8) / (r1 + r2));
_Pc2->setValue(2, (r1 * q_2 + r2 * q_9) / (r1 + r2));
_Nc->setValue(0, (q_0 - q_7) / (y.getValue(0) + r1pr2));
_Nc->setValue(1, (q_1 - q_8) / (y.getValue(0) + r1pr2));
_Nc->setValue(2, (q_2 - q_9) / (y.getValue(0) + r1pr2));
//std::cout<<" SphereNEDSSphereNEDSR::computeh dist:"<<y->getValue(0)<<"\n";
//std::cout<<"_Pc:\n";
//_Pc->display();
//std::cout<<"_Nc:\n";
//_Nc->display();
}
static
void normalize(SiconosVector& q, unsigned int i)
{
q.setValue(i, fmod(q.getValue(i), _2PI));
assert(fabs(q.getValue(i)) - std::numeric_limits<double>::epsilon() >= 0.);
assert(fabs(q.getValue(i)) < _2PI);
}
void BulletR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
DEBUG_BEGIN("BulletR::computeh(...)\n");
NewtonEulerR::computeh(time, q0, y);
DEBUG_PRINT("start of computeh\n");
btVector3 posa = _contactPoints->getPositionWorldOnA();
btVector3 posb = _contactPoints->getPositionWorldOnB();
if (_flip) {
posa = _contactPoints->getPositionWorldOnB();
posb = _contactPoints->getPositionWorldOnA();
}
(*pc1())(0) = posa[0];
(*pc1())(1) = posa[1];
(*pc1())(2) = posa[2];
(*pc2())(0) = posb[0];
(*pc2())(1) = posb[1];
(*pc2())(2) = posb[2];
{
y.setValue(0, _contactPoints->getDistance());
(*nc())(0) = _contactPoints->m_normalWorldOnB[0] * (_flip ? -1 : 1);
(*nc())(1) = _contactPoints->m_normalWorldOnB[1] * (_flip ? -1 : 1);
(*nc())(2) = _contactPoints->m_normalWorldOnB[2] * (_flip ? -1 : 1);
}
DEBUG_PRINTF("distance : %g\n", y.getValue(0));
DEBUG_PRINTF("position on A : %g,%g,%g\n", posa[0], posa[1], posa[2]);
DEBUG_PRINTF("position on B : %g,%g,%g\n", posb[0], posb[1], posb[2]);
DEBUG_PRINTF("normal on B : %g,%g,%g\n", (*nc())(0), (*nc())(1), (*nc())(2));
DEBUG_END("BulletR::computeh(...)\n");
}