//assumes that the Vectors are normalized
hsBool ThreePlaneIntersect(const NxVec3& norm0, const NxVec3& point0,
const NxVec3& norm1, const NxVec3& point1,
const NxVec3& norm2, const NxVec3& point2, NxVec3& loc)
{
//need to make sure these planes aren't parallel
hsBool suc=0;
NxVec3 cross=norm1.cross( norm2);
float denom=norm0.dot(cross);
if(abs(denom)<0.0001) return 0;//basically paralell
// if we are here there must be a point in 3 space
try{
float d1,d2,d3;
d1=norm0.dot(point0);
d2=norm1.dot(point1);
d3=norm2.dot(point2);
NxVec3 n1Xn2=norm1.cross(norm2);
NxVec3 n2Xn0=norm2.cross(norm0);
NxVec3 n0Xn1=norm0.cross(norm1);
NxVec3 pos=(d1*n1Xn2+ d2*n2Xn0 + d3*n0Xn1)/(denom);
loc.x=pos.x;
loc.y=pos.y;
loc.z=pos.z;
suc= 1;
}
catch(...)
{
suc=0;
}
return suc;
}
void MotionCallback(int x, int y)
{
int dx = mx - x;
int dy = my - y;
if (gMouseSphere) // Move the mouse sphere
{
NxVec3 pos;
ViewUnProject(x,y, gMouseDepth, pos);
gMouseSphere->setGlobalPosition(pos);
gHitActor->wakeUp();
}
else if (gHitCloth) // Attach the cloth vertex
{
NxVec3 pos;
ViewUnProject(x,y, gMouseDepth, pos);
gHitCloth->attachVertexToGlobalPosition(gHitClothVertex, pos);
}
else if (bLeftMouseButtonPressed) // Set camera
{
gCameraForward.normalize();
gCameraRight.cross(gCameraForward,NxVec3(0,1,0));
NxQuat qx(NxPiF32 * dx * 20 / 180.0f, NxVec3(0,1,0));
qx.rotate(gCameraForward);
NxQuat qy(NxPiF32 * dy * 20 / 180.0f, gCameraRight);
qy.rotate(gCameraForward);
}
mx = x;
my = y;
}
void DXApp::Frame()
{
UINT64 CurrentTime;
UINT64 DeltaCount;
QueryPerformanceCounter((LARGE_INTEGER*)&CurrentTime);
DeltaCount = CurrentTime - OldCount;
OldCount = CurrentTime;
DeltaTime = (double)DeltaCount/(double)Frequency;
{
NxVec3 Temp;
Cam.location+=Cam.ViewDir*40*fFoward;
Temp = Cam.ViewDir;
Temp.y = 0;
Temp.normalize();
Temp = Temp.cross(NxVec3(0.0f, 1.0f, 0.0f));
Cam.location+=Temp*40*fStrafe;;
}
if(!bPaused)
{
DoParticles();
_p_scene->simulate(_time_step);
_p_scene->flushStream();
}
RenderFrame();
if(!bPaused)
_p_scene->fetchResults(NX_RIGID_BODY_FINISHED, true);
}
static void MotionCallback(int x, int y)
{
int dx = gMouseX - x;
int dy = gMouseY - y;
gDir.normalize(); //カメラの視線ベクトルを正規化
gViewY.cross(gDir, NxVec3(0,1,0)); //
if( gMouseButton[0] && gMouseButton[1] ) {
//Zoom: Left + Center Buttons Drag
gEye -= gDir * 0.5f * dy;
} else {
if( gMouseButton[0] ) {
//Rotate: Left Button Drag
NxQuat qx(NxPiF32 * dx * 10/ 180.0f, NxVec3(0,1,0));
qx.rotate(gDir);
NxQuat qy(NxPiF32 * dy * 10/ 180.0f, gViewY);
qy.rotate(gDir);
} else if( gMouseButton[1] ) {
//Move: Center Button Drag
gEye += 0.1f * (gViewY * dx - NxVec3(0, 1, 0) * dy);
}
}
gMouseX = x;
gMouseY = y;
glutPostRedisplay();
}
void MotionCallback(int x, int y)
{
int dx = mx - x;
int dy = my - y;
gCameraForward.normalize();
gCameraRight.cross(gCameraForward,NxVec3(0,1,0));
NxQuat qx(NxPiF32 * dx * 20 / 180.0f, NxVec3(0,1,0));
qx.rotate(gCameraForward);
NxQuat qy(NxPiF32 * dy * 20 / 180.0f, gCameraRight);
qy.rotate(gCameraForward);
mx = x;
my = y;
}
static void MotionCallback(int x, int y)
{
int dx = mx - x;
int dy = my - y;
Dir.normalize();
N.cross(Dir,NxVec3(0,1,0));
NxQuat qx(NxPiF32 * dx * 20/ 180.0f, NxVec3(0,1,0));
qx.rotate(Dir);
NxQuat qy(NxPiF32 * dy * 20/ 180.0f, N);
qy.rotate(Dir);
mx = x;
my = y;
}
void MouseMotion(int x, int y )
{
if(g_isButtonDown[0] || g_isButtonDown[2])
{
int dx = g_last_x - x;
int dy = g_last_y - y;
g_CameraForward.normalize();
g_CameraRight.cross(g_CameraForward, NxVec3(0,1,0));
NxQuat qx(NxPiF32 * dx * 20/ 180.0f, NxVec3(0,1,0));
qx.rotate(g_CameraForward);
NxQuat qy(NxPiF32 * dy * 20/ 180.0f, g_CameraRight);
qy.rotate(g_CameraForward);
g_last_x = x;
g_last_y = y;
}
if (g_isButtonDown[1])
{
float dt = 0.1f;
if ((float) (x-g_last_x)>0)
{
g_CameraPos -= g_CameraRight*g_Speed*dt;
}
else if ((float) (x-g_last_x)<0)
{
g_CameraPos += g_CameraRight*g_Speed*dt;
}
if ((float) (y-g_last_y)>0)
{
g_CameraPos +=NxVec3(0.0f,1.0f,0.0f)*g_Speed*dt;
}
else if ((float) (y-g_last_y)<0)
{
g_CameraPos -=NxVec3(0.0f,1.0f,0.0f)*g_Speed*dt;
}
g_last_x = x;
g_last_y = y;
}
glutPostRedisplay();
}
void DXApp::HandleMouseMove(bool bMouseDown, int X, int Y)
{
if(bMouseDown)
{
NxVec3 Temp;
Temp = Cam.location;
Temp.y = 0;
Temp.normalize();
Temp = Temp.cross(NxVec3(0.0f, 1.0f, 0.0f));
NxQuat(((float)(Y-OldMY))/20.0f, Temp).rotate(Cam.ViewDir);
Temp = NxVec3(0.0f, 1.0f, 0.0f);
NxQuat(((float)(X-OldMX))/20.0f, Temp).rotate(Cam.ViewDir);
}
OldMX = X;
OldMY = Y;
}
// ----------------------------------------------------------------------
void ObjMesh::updateNormals()
{
mNormals.resize(mVertices.size());
int i;
for (i = 0; i < (int)mNormals.size(); i++)
mNormals[i].zero();
NxVec3 n;
for (i = 0; i < (int)mTriangles.size(); i++) {
ObjMeshTriangle &mt = mTriangles[i];
mt.normalNr[0] = mt.vertexNr[0];
mt.normalNr[1] = mt.vertexNr[1];
mt.normalNr[2] = mt.vertexNr[2];
n.cross(mVertices[mt.vertexNr[1]] - mVertices[mt.vertexNr[0]],
mVertices[mt.vertexNr[2]] - mVertices[mt.vertexNr[0]]);
mNormals[mt.normalNr[0]] += n;
mNormals[mt.normalNr[1]] += n;
mNormals[mt.normalNr[2]] += n;
}
for (i = 0; i < (int)mNormals.size(); i++)
mNormals[i].normalize();
}
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");
}
