void DYN_initialize(DYN_Context *context, double timeStep)
{
context->bodies = 0;
context->bodiesAllocated = 0;
context->timeStep = timeStep;
context->elapsedTime = 0;
if (!DYN_log)
{
DYN_log = fopen("log.txt", "wt");
}
DYNA_initialize(&context->nonCollidingPairs, sizeof( int[2] ));
DYNA_initialize(&context->collidingPairs, sizeof( int[2] ));
AVL_initialize(&context->ncPairIndex, compareBodyIndexPair);
}
void Dynamixel::connect(){ // Initiallizes the motor and generates the ftHandleDYNA
FT_STATUS ftStatus = FT_OpenEx(serialNumber_,FT_OPEN_BY_SERIAL_NUMBER,&ftHandleDYNA_);
if (ftStatus != FT_OK)
{
qDebug()<<"failed to open dyna";
emit failedToOpen();
Sleep(2000);
return;
}
//Initialize the USB2Dynamixel
initialized_=DYNA_initialize(ftHandleDYNA_);
if(!initialized_){
qDebug()<<"failed to initialize";
emit failedToOpen();
return;
}
Sleep(50);
qDebug()<< "Opend Dynamixel!";
}
/**
* Steps the world.
*
* This function is not complete, may need some rework.
*/
void DYN_stepWorld(DYN_Context *context)
{
int i, j;
double remainingTime = 1;
double nearestPoints[6];
int stuckCtr = 0;
if (simStopped) return;
while (remainingTime > 0)
{
char wasCollision = 0;
// STEP 1: Moving bodies
for (i = 0; i < context->bodyCount; i++)
{
moveBody(&context->bodies[i], remainingTime);
}
// STEP 2: Collision detection
for (i = 0; i < context->bodyCount; i++)
{
context->bodies[i].colliding = 0;
context->bodies[i].inContact = 0;
}
for (i = 0; i < context->bodyCount; i++)
{
for (j = i + 1; j < context->bodyCount; j++)
{
double nearestPoints[6];
int tmp[2];
tmp[0] = i;
tmp[1] = j;
if (AVL_find(&context->ncPairIndex, tmp))
{
continue;
}
if (COL_collide(&context->bodies[i], &context->bodies[j], nearestPoints, 0))
{
addCollidingPair(context, i, j);
wasCollision = 1;
}
}
}
if (wasCollision)
{
double currentImpulseVector[3];
double currentCollisionPoint[3];
double minTime = 1;
int earlyAIndex = -1, earlyBIndex = -1;
// There was collision
// STEP 3: Find earliest collision
int i;
int cpCount = DYNA_getLength(&context->collidingPairs);
int (*collidingPairs)[2] = DYNA_getStorage(&context->collidingPairs);
// Find the earliest collision.
assert(cpCount);
for (i = 0; i < cpCount; i++)
{
DYN_Body *a = &context->bodies[collidingPairs[i][0]];
DYN_Body *b = &context->bodies[collidingPairs[i][1]];
double lower = 0;
double upper = remainingTime;
int stuckCtr2 = 0;
char nearestSet = 0;
for(;;stuckCtr2++)
{
double middle = (lower + upper) * 0.5;
revertMovement(a);
revertMovement(b);
moveBody(a, middle);
moveBody(b, middle);
if (COL_collide(a, b, nearestPoints, 0))
{
upper = middle;
}
else
{
nearestSet = 1;
lower = middle;
}
assert(lower != upper);
if (nearestSet)
{
double tmp[3];
ALG_getPointToPointVector(tmp, nearestPoints, &nearestPoints[3]);
if ((ALG_dotProduct(tmp, tmp) < 1e-6) || (fabs(upper-lower) < 1e-6))
{
// The two bodies are near each other.
if (middle <= minTime)
{
minTime = middle;
earlyAIndex = collidingPairs[i][0];
earlyBIndex = collidingPairs[i][1];
memcpy(currentImpulseVector, tmp, sizeof(currentImpulseVector));
memcpy(currentCollisionPoint, nearestPoints, sizeof(currentCollisionPoint));
}
break;
}
}
}
}
{
DYN_Body *a = &context->bodies[earlyAIndex];
DYN_Body *b = &context->bodies[earlyBIndex];
fprintf(DYN_log, "Collision!\n");
fprintf(DYN_log, "Body index A: %d\n", earlyAIndex);
fprintf(DYN_log, "Body index B: %d\n", earlyBIndex);
fprintf(DYN_log, "Velocity A [%g, %g, %g] %g\n", a->velocity[0], a->velocity[1], a->velocity[2], ALG_getVectorLength(a->velocity));
fprintf(DYN_log, "Velocity B [%g, %g, %g] %g\n", b->velocity[0], b->velocity[1], b->velocity[2], ALG_getVectorLength(b->velocity));
fprintf(DYN_log, "Collision point [%g, %g, %g]\n", currentCollisionPoint[0], currentCollisionPoint[1], currentCollisionPoint[2]);
fprintf(
DYN_log,
"Impulse vector [%g, %g, %g] %g\n",
currentImpulseVector[0],
currentImpulseVector[1],
currentImpulseVector[2],
ALG_getVectorLength(currentImpulseVector)
);
fprintf(DYN_log, "Elapsed time: %g\n", context->elapsedTime + context->timeStep * (1 - remainingTime));
}
// Revert and move all bodies to the collision moment.
for (i = 0; i < context->bodyCount; i++)
{
DYN_Body *current = &context->bodies[i];
revertMovement(current);
moveBody(current, minTime);
}
assert(earlyAIndex >= 0);
assert(earlyBIndex >= 0);
// Resolve the collision
{
CollisionResult result = resolveCollision(
context,
&context->bodies[earlyAIndex],
&context->bodies[earlyBIndex],
currentCollisionPoint,
currentImpulseVector
);
if (result == DYN_STICKEDTOGETHER)
{
addNonCollidingPair(context, earlyAIndex, earlyBIndex);
context->bodies[earlyAIndex].inContact = 1;
context->bodies[earlyBIndex].inContact = 1;
}
}
memcpy(DYN_lastCollisionPoint, currentCollisionPoint, sizeof(DYN_lastCollisionPoint));
memcpy(DYN_lastImpulse, currentImpulseVector, sizeof(DYN_lastImpulse));
fprintf(DYN_log, "==============================\n");
//
clearCollidingPairs(context);
// Continue the simulation
remainingTime -= minTime;
}
else
{
// No collision, step finished
break;
}
stuckCtr++;
if (stuckCtr >= 100)
{
simStopped = 1;
return;
}
}
// STEP 4: repelling non-colliding pairs
{
int i;
int n = DYNA_getLength(&context->nonCollidingPairs);
int (*ncPairs)[2] = (int (*)[2])DYNA_getStorage(&context->nonCollidingPairs);
double lastSimplex[12];
for (i = 0; i < n; i++)
{
DYN_Body *a = &context->bodies[ncPairs[i][0]];
DYN_Body *b = &context->bodies[ncPairs[i][1]];
if (COL_collide(a, b, 0, lastSimplex))
{
double penetrationVector[3];
fprintf(DYN_log, "Sticked and colliding after timestep!\n");
COL_getPenetrationVector(a, b, lastSimplex, penetrationVector);
fprintf(
DYN_log,
"Penetration vector was: [%g, %g, %g]\n",
penetrationVector[0],
penetrationVector[1],
penetrationVector[2]
);
{
// push it out from the body and a little.
double penetrationVectorLength = ALG_getVectorLength(penetrationVector);
double scaleFactor = (penetrationVectorLength + 0.01) / penetrationVectorLength;
ALG_scale(penetrationVector, scaleFactor);
}
ALG_translate(b->position, penetrationVector);
fprintf(DYN_log, "==============================\n");
a->colliding = a->inContact = 0;
b->colliding = a->inContact = 0;
{
// Body b may collide with other bodies. Translate those bodies as well.
DYNA_Array tmp;
int j;
DYNA_initialize(&tmp, sizeof(DYN_Body*));
DYNA_add(&tmp, b);
for (j = 0; j < DYNA_getLength(&tmp); j++)
{
DYN_Body **storage = DYNA_getStorage(&tmp);
DYN_Body *current = storage[j];
int k = 0;
for (k = 0; k < context->bodyCount; k++)
{
DYN_Body *other = &context->bodies[k];
if (other == current) continue;
if (COL_collide(current, other, 0, 0))
{
ALG_translate(other->position, penetrationVector);
current->colliding = 0;
other->colliding = 0;
DYNA_add(&tmp, other);
}
}
}
DYNA_deinitialize(&tmp);
}
}
}
}
clearNonCollidingPairs(context);
context->elapsedTime += context->timeStep;
}
