int CLocalConnection::SendData(const unsigned char *data, const unsigned length)
{
boost::mutex::scoped_lock scoped_lock(Mutex[otherInstance()]);
if(active){
if(Length[otherInstance()]+length>=NETWORK_BUFFER_SIZE){
logOutput.Print("Overflow when sending to local connection: Outbuf: %i Data: %i BuffSize: %i",Length[otherInstance()],length,NETWORK_BUFFER_SIZE);
return 0;
}
memcpy(&Data[otherInstance()][Length[otherInstance()]],data,length);
Length[otherInstance()]+=length;
dataSent += length;
}
return 1;
}
void dgCollisionScene::CollideCompoundPair (dgCollidingPairCollector::dgPair* const pair, dgCollisionParamProxy& proxy) const
{
const dgNodeBase* stackPool[4 * DG_COMPOUND_STACK_DEPTH][2];
dgContact* const constraint = pair->m_contact;
dgBody* const myBody = constraint->GetBody1();
dgBody* const otherBody = constraint->GetBody0();
dgAssert (myBody == proxy.m_floatingBody);
dgAssert (otherBody == proxy.m_referenceBody);
dgCollisionInstance* const myCompoundInstance = myBody->m_collision;
dgCollisionInstance* const otherCompoundInstance = otherBody->m_collision;
dgAssert (myCompoundInstance->GetChildShape() == this);
dgAssert (otherCompoundInstance->IsType (dgCollision::dgCollisionCompound_RTTI));
dgCollisionCompound* const otherCompound = (dgCollisionCompound*)otherCompoundInstance->GetChildShape();
const dgContactMaterial* const material = constraint->GetMaterial();
dgMatrix myMatrix (myCompoundInstance->GetLocalMatrix() * myBody->m_matrix);
dgMatrix otherMatrix (otherCompoundInstance->GetLocalMatrix() * otherBody->m_matrix);
dgOOBBTestData data (otherMatrix * myMatrix.Inverse());
dgInt32 stack = 1;
stackPool[0][0] = m_root;
stackPool[0][1] = otherCompound->m_root;
const dgVector& hullVeloc = otherBody->m_veloc;
dgFloat32 baseLinearSpeed = dgSqrt (hullVeloc % hullVeloc);
dgFloat32 closestDist = dgFloat32 (1.0e10f);
if (proxy.m_continueCollision && (baseLinearSpeed > dgFloat32 (1.0e-6f))) {
dgAssert (0);
} else {
while (stack) {
stack --;
const dgNodeBase* const me = stackPool[stack][0];
const dgNodeBase* const other = stackPool[stack][1];
dgAssert (me && other);
if (me->BoxTest (data, other)) {
if ((me->m_type == m_leaf) && (other->m_type == m_leaf)) {
dgAssert (!me->m_right);
bool processContacts = true;
if (material->m_compoundAABBOverlap) {
processContacts = material->m_compoundAABBOverlap (*material, myBody, me->m_myNode, otherBody, other->m_myNode, proxy.m_threadIndex);
}
if (processContacts) {
const dgCollisionInstance* const mySrcInstance = me->GetShape();
const dgCollisionInstance* const otherSrcInstance = other->GetShape();
//dgCollisionInstance childInstance (*mySrcInstance, mySrcInstance->GetChildShape());
//dgCollisionInstance otherInstance (*otherSrcInstance, otherSrcInstance->GetChildShape());
dgCollisionInstance childInstance (*me->GetShape(), me->GetShape()->GetChildShape());
dgCollisionInstance otherInstance (*other->GetShape(), other->GetShape()->GetChildShape());
childInstance.SetGlobalMatrix(childInstance.GetLocalMatrix() * myMatrix);
otherInstance.SetGlobalMatrix(otherInstance.GetLocalMatrix() * otherMatrix);
proxy.m_floatingCollision = &childInstance;
proxy.m_referenceCollision = &otherInstance;
dgInt32 count = pair->m_contactCount;
m_world->SceneChildContacts (pair, proxy);
if (pair->m_contactCount > count) {
dgContactPoint* const buffer = proxy.m_contacts;
for (dgInt32 i = count; i < pair->m_contactCount; i ++) {
//if (buffer[i].m_collision0 == proxy.m_floatingCollision) {
// buffer[i].m_collision0 = mySrcInstance;
//}
//if (buffer[i].m_collision1 == proxy.m_referenceCollision) {
// buffer[i].m_collision1 = otherSrcInstance;
//}
if (buffer[i].m_collision1->GetChildShape() == otherSrcInstance->GetChildShape()) {
dgAssert(buffer[i].m_collision0->GetChildShape() == mySrcInstance->GetChildShape());
buffer[i].m_collision0 = mySrcInstance;
buffer[i].m_collision1 = otherSrcInstance;
} else {
dgAssert(buffer[i].m_collision1->GetChildShape() == mySrcInstance->GetChildShape());
buffer[i].m_collision1 = mySrcInstance;
buffer[i].m_collision0 = otherSrcInstance;
}
}
}
closestDist = dgMin(closestDist, constraint->m_closestDistance);
}
} else if (me->m_type == m_leaf) {
dgAssert (other->m_type == m_node);
stackPool[stack][0] = me;
stackPool[stack][1] = other->m_left;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack][0] = me;
stackPool[stack][1] = other->m_right;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
} else if (other->m_type == m_leaf) {
dgAssert (me->m_type == m_node);
stackPool[stack][0] = me->m_left;
stackPool[stack][1] = other;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack][0] = me->m_right;
stackPool[stack][1] = other;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
} else {
dgAssert (me->m_type == m_node);
dgAssert (other->m_type == m_node);
stackPool[stack][0] = me->m_left;
stackPool[stack][1] = other->m_left;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack][0] = me->m_left;
stackPool[stack][1] = other->m_right;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack][0] = me->m_right;
stackPool[stack][1] = other->m_left;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
stackPool[stack][0] = me->m_right;
stackPool[stack][1] = other->m_right;
stack++;
dgAssert (stack < dgInt32 (sizeof (stackPool) / sizeof (dgNodeBase*)));
}
}
}
}
constraint->m_closestDistance = closestDist;
}
