int pWorld::overlapOBBShapes(const VxBbox& worldBounds, CK3dEntity*shapeReference, pShapesType shapeType,CKGroup *shapes,int activeGroups/* =0xffffffff */, const pGroupsMask* groupsMask/* =NULL */, bool accurateCollision/* =false */)
{
int result=0;
NxBox box;
if (shapeReference)
{
NxShape *shape = getShapeByEntityID(shapeReference->GetID());
if (shape)
{
//shape->checkOverlapAABB()
NxBoxShape*boxShape = static_cast<NxBoxShape*>(shape->isBox());
if (boxShape)
{
boxShape->getWorldOBB(box);
}
}
}else{
box.center = getFrom(worldBounds.GetCenter());
box.extents = getFrom(worldBounds.GetSize());
}
int total = 0;
if (shapeType & ST_Dynamic )
{
total+=getScene()->getNbDynamicShapes();
}
if (shapeType & ST_Static)
{
total+=getScene()->getNbStaticShapes();
}
NxShape** _shapes = (NxShape**)NxAlloca(total*sizeof(NxShape*));
for (NxU32 i = 0; i < total; i++) _shapes[i] = NULL;
NxGroupsMask mask;
if (groupsMask)
{
mask.bits0 = groupsMask->bits0;
mask.bits1 = groupsMask->bits1;
mask.bits2 = groupsMask->bits2;
mask.bits3 = groupsMask->bits3;
}else{
mask.bits0 = 0;
mask.bits1 = 0;
mask.bits2 = 0;
mask.bits3 = 0;
}
result = getScene()->overlapOBBShapes(box,(NxShapesType)shapeType,total,_shapes,NULL,activeGroups,&mask,accurateCollision);
if (_shapes && shapes )
{
for (int i = 0 ; i < result ; i++)
{
NxShape *s = _shapes[i];
if (s)
{
const char* name =s->getName();
pSubMeshInfo *sInfo = static_cast<pSubMeshInfo*>(s->userData);
if (sInfo->entID)
{
CKObject *obj = (CKObject*)GetPMan()->m_Context->GetObject(sInfo->entID);
if (obj)
{
shapes->AddObject((CKBeObject*)obj);
}
}
}
}
}
int op=2;
return result;
}
int pWorld::overlapSphereShapes(const VxSphere& worldSphere,CK3dEntity*shapeReference,pShapesType shapeType,CKGroup*shapes,int activeGroups/* =0xffffffff */, const pGroupsMask* groupsMask/* =NULL */, bool accurateCollision/* =false */)
{
int result=0;
NxSphere sphere;
if (shapeReference)
{
NxShape *shape = getShapeByEntityID(shapeReference->GetID());
if (shape)
{
//shape->checkOverlapAABB()
NxSphereShape *sphereShape = static_cast<NxSphereShape*>(shape->isSphere());
if (sphereShape)
{
sphere.radius = sphereShape->getRadius() + worldSphere.Radius();
//ori :
VxVector ori = worldSphere.Center();
VxVector oriOut = ori;
if (shapeReference)
{
shapeReference->Transform(&oriOut,&ori);
}
sphere.center = getFrom(oriOut);
}
}
}else{
sphere.center = getFrom(worldSphere.Center());
sphere.radius = worldSphere.Radius();
}
int total = 0;
if (shapeType & ST_Dynamic )
{
total+=getScene()->getNbDynamicShapes();
}
if (shapeType & ST_Static)
{
total+=getScene()->getNbStaticShapes();
}
NxShape** _shapes = (NxShape**)NxAlloca(total*sizeof(NxShape*));
for (NxU32 i = 0; i < total; i++) _shapes[i] = NULL;
NxGroupsMask mask;
if (groupsMask)
{
mask.bits0 = groupsMask->bits0;
mask.bits1 = groupsMask->bits1;
mask.bits2 = groupsMask->bits2;
mask.bits3 = groupsMask->bits3;
}else{
mask.bits0 = 0;
mask.bits1 = 0;
mask.bits2 = 0;
mask.bits3 = 0;
}
result = getScene()->overlapSphereShapes(sphere,(NxShapesType)shapeType,total,_shapes,NULL,activeGroups,&mask,accurateCollision);
if (_shapes && shapes )
{
for (int i = 0 ; i < result ; i++)
{
NxShape *s = _shapes[i];
if (s)
{
const char* name =s->getName();
pSubMeshInfo *sInfo = static_cast<pSubMeshInfo*>(s->userData);
if (sInfo->entID)
{
CKObject *obj = (CKObject*)GetPMan()->m_Context->GetObject(sInfo->entID);
if (obj)
{
shapes->AddObject((CKBeObject*)obj);
}
}
}
}
}
int op=2;
return result;
}
void NxRobot::handleContactPair(NxContactPair& pair, NxU32 robotIndex)
{
NxContactStreamIterator i(pair.stream);
while(i.goNextPair())
{
NxShape * s = i.getShape(robotIndex); //umas das partes do robo (o robo eh um unico actor com as MESHs: rodas, driblador, corpo)
NxShape * s1 = i.getShape(1-robotIndex); //corpo que o robo esta colidindo
while(i.goNextPatch())
{
const NxVec3& contactNormal = i.getPatchNormal();
while(i.goNextPoint())
{
//user can also call getPoint() and getSeparation() here
const NxVec3& contactPoint = i.getPoint();
//add forces:
/*//assuming front wheel drive we need to apply a force at the wheels.
if (s->is(NX_SHAPE_CAPSULE) && s->userData != NULL) {
//assuming only the wheels of the car are capsules, otherwise we need more checks.
//this branch can't be pulled out of loops because we have to do a full iteration through the stream
NxQuat local2global = s->getActor().getGlobalOrientationQuat();
NxWheel* w = (NxWheel*)s->userData;
if (!w->getWheelFlag(NX_WF_USE_WHEELSHAPE))
{
NxWheel1 * wheel = static_cast<NxWheel1*>(w);
wheel->contactInfo.otherActor = pair.actors[1-robotIndex];
wheel->contactInfo.contactPosition = contactPoint;
wheel->contactInfo.contactPositionLocal = contactPoint;
wheel->contactInfo.contactPositionLocal -= this->getActor()->getGlobalPosition();
local2global.inverseRotate(wheel->contactInfo.contactPositionLocal);
wheel->contactInfo.contactNormal = contactNormal;
if (wheel->contactInfo.otherActor->isDynamic())
{
NxVec3 globalV = s->getActor().getLocalPointVelocity(wheel->getWheelPos());
globalV -= wheel->contactInfo.otherActor->getLinearVelocity();
local2global.inverseRotate(globalV);
wheel->contactInfo.relativeVelocity = globalV.x;
//printf("%2.3f (%2.3f %2.3f %2.3f)\n", wheel->contactInfo.relativeVelocity,
// globalV.x, globalV.y, globalV.z);
}
else
{
NxVec3 vel = s->getActor().getLocalPointVelocity(wheel->getWheelPos());
local2global.inverseRotate(vel);
wheel->contactInfo.relativeVelocity = vel.x;
wheel->contactInfo.relativeVelocitySide = vel.z;
}
NX_ASSERT(wheel->hasGroundContact());
//printf(" Wheel %x is touching\n", wheel);
}
}*/
const char* name = s->getName();
if(name) {
const char* name1 = s1->getName();
if(name1) {
char* dribblerName = new char[10]; //"Driblador\0"
dribblerName[9] = 0;
memcpy(dribblerName, name, strlen(dribblerName));
char* ballName = new char[5]; //"Bola\0"
ballName[4] = 0;
memcpy(ballName, name1, strlen(ballName));
if(strcmp(dribblerName, "Driblador")==0 && strcmp(ballName, "Bola")==0) {
float angle = this->getAngle2DFromVehicle();
angle += NxPi/2.;
NxActor& ball = s1->getActor();
NxVec3 dribblerGlobalPosition = this->dribbler->getLocalPosition() + this->getPos();
NxVec3 r = contactPoint - dribblerGlobalPosition;
NxVec3 w = NxVec3(cos(angle), sin(angle), 0);
NxVec3 force = w.cross(r);
force.setMagnitude(1);
NxReal coefKinect = 0.5;
NxReal normalMagnitude = contactNormal.magnitude();
NxVec3 resultForce = this->dribbler->speedToExecute * 20. * coefKinect * normalMagnitude * force;
//NxReal teste = resultForce.magnitude();
//SimulationView::DrawForce(&ball, resultForce, NxVec3(1,1,1));
ball.addForceAtPos(/*NxVec3(sin(angle)*this->dribbler->speedToExecute*1000000.,cos(angle)*this->dribbler->speedToExecute*1000000.,0)*/resultForce, contactPoint, NX_IMPULSE);
this->dribbler->speedToExecute = 0;
}
delete dribblerName;
delete ballName;
}
}
}
}
}
//printf("----\n");
}