void VehicleController::resetPose()
{
NxQuat quat;
quat.x = 0;
quat.y = 1;
quat.z = 0;
quat.w = 0;
NxMat33 rot(quat);
m_actor->setGlobalPose(NxMat34(rot, m_actor->getGlobalPosition() + NxVec3(0, 5, 0)));
}
void Vehicle::resetCarPos()
{
NxVec3 vec3(mOriginalPos.x, mOriginalPos.y, mOriginalPos.z);
NxQuat quat;
quat.setWXYZ(mOriginalQuat.w, mOriginalQuat.x, mOriginalQuat.y, mOriginalQuat.z);
mActor->setGlobalPose(NxMat34(quat, vec3));
vec3.zero();
mActor->setLinearVelocity(vec3);
mActor->setAngularVelocity(vec3);
}
void Simulation::buildModelBall(int indexScene){
NxBall* ball = Simulation::gScenes[indexScene]->ball;
ball->ball = Simulation::getActorBall(indexScene);
//velocidade maxima permitida segundo as rule 2010 (10m/s)
//A bola pode atingir velocidade maior pq tem o caso de esta sendo usada pelo driblador, mas a principio consideremos isso
ball->ball->setMaxAngularVelocity(10000./21.5);
//0.031 medido da bola do lab
//46g a bola da rules 2010
//estimativa da bola do laboratorio 31g
ball->ball->setMass(0.046); //PLUGIN TAH COM PROBLEMA XML ERRADO
//TODO: LEVANTAR INERTIA TENSOR, CMASS, DAMPINGS
//float teste = ball->ball->getAngularDamping();
//ball->ball->setCMassOffsetGlobalPosition(NxVec3(0, 0, 0));
ball->ball->setCMassOffsetLocalPose( NxMat34( NxMat33(NxVec3(8.37673, 0, 0), NxVec3(0, 8.37673, 0), NxVec3(0, 0, 8.37673)), NxVec3(0, 0, 0) ) );
ball->ball->setAngularDamping(0.5);
ball->ball->setLinearDamping(0.5);
ball->ball->setMassSpaceInertiaTensor(/*ball->ball->getMassSpaceInertiaTensor()*100000.*/ NxVec3(8.37673, 8.37673, 8.37673) );
ball->initialPose = ball->ball->getGlobalPose();
ball->indexScene = indexScene;
ball->ball->putToSleep();
}
void World::SetupSoftWheelCarScene()
{
NxSoftBodyDesc softBodyDesc;
softBodyDesc.particleRadius = 0.2f;
softBodyDesc.volumeStiffness = 1.0f;
softBodyDesc.stretchingStiffness = 1.0f;
softBodyDesc.friction = 1.0f;
softBodyDesc.attachmentResponseCoefficient = 0.8f;
softBodyDesc.tearFactor = 10.1; // should be more than 1.0, the less, the easier to tear with flag NX_SBF_TEARABLE
softBodyDesc.flags |= NX_SBF_HARDWARE | NX_SBF_VOLUME_CONSERVATION | NX_SBF_TEARABLE;
char *fileName = "wheel";
char tetFileName[256], objFileName[256], s[256];
sprintf(tetFileName, "%s.tet", fileName);
sprintf(objFileName, "%s.obj", fileName);
MySoftBody *softBody;
NxReal carHeight = 7.5f;
NxReal stiffness = 0.9f;
NxMat34 capsulePose = NxMat34(NxMat33(NX_IDENTITY_MATRIX), NxVec3(-4, carHeight, -5.0f));
printf("capsulePose %f %f %f\n", capsulePose.t.x, capsulePose.t.y, capsulePose.t.z);
capsulePose.M.rotX(NxHalfPiF32);
NxActor *caps1 = CreateOrientedCapsule(capsulePose, 7.1f, 1.3f, 1.0f);
capsulePose.t = NxVec3(4, carHeight, -5.0f);
NxActor *caps2 = CreateOrientedCapsule(capsulePose, 7.1f, 1.3f, 1.0f);
ObjMesh *objMesh = new ObjMesh();
objMesh->loadFromObjFile(FindMediaFile(objFileName, s));
NxMat33 rot;
rot.rotX(NxPiF32);
softBodyDesc.globalPose.t = NxVec3(4.0f, carHeight, 3.4f);
softBodyDesc.globalPose.M = rot;
softBodyDesc.stretchingStiffness = stiffness;
softBody = new MySoftBody(gScene, softBodyDesc, FindMediaFile(tetFileName,s), objMesh);
if (!softBody->getNxSoftBody())
{
printf("Error: Unable to create Softbody for the current scene.\n");
delete softBody;
}
else
{
gObjMeshes.push_back(objMesh);
// wheel 1
softBody->getNxSoftBody()->attachToCollidingShapes(NX_SOFTBODY_ATTACHMENT_TWOWAY);
gSoftBodies.push_back(softBody);
softBodyDesc.globalPose.t = NxVec3(-4.0f ,carHeight, 3.4f);
softBodyDesc.globalPose.M = rot;
softBodyDesc.stretchingStiffness = stiffness;
softBody = new MySoftBody(gScene, softBodyDesc, FindMediaFile(tetFileName,s), objMesh);
softBody->getNxSoftBody()->attachToCollidingShapes(NX_SOFTBODY_ATTACHMENT_TWOWAY);
gSoftBodies.push_back(softBody);
softBodyDesc.globalPose.t = NxVec3(4.0f, carHeight, -3.4f);
softBodyDesc.globalPose.M.id();
softBodyDesc.stretchingStiffness = stiffness;
softBody = new MySoftBody(gScene, softBodyDesc, FindMediaFile(tetFileName,s), objMesh);
softBody->getNxSoftBody()->attachToCollidingShapes(NX_SOFTBODY_ATTACHMENT_TWOWAY);
gSoftBodies.push_back(softBody);
softBodyDesc.globalPose.t = NxVec3(-4.0f, carHeight, -3.4f);
softBodyDesc.globalPose.M.id();
softBodyDesc.stretchingStiffness = stiffness;
softBody = new MySoftBody(gScene, softBodyDesc, FindMediaFile(tetFileName,s), objMesh);
softBody->getNxSoftBody()->attachToCollidingShapes(NX_SOFTBODY_ATTACHMENT_TWOWAY); // bind soft wheel with capsule colliding with it
gSoftBodies.push_back(softBody);
NxActor *box = CreateBox(NxVec3(0,carHeight,0), NxVec3(4.6f, 0.5f, 1.0f), 0,1.0f);
CreateRevoluteJoint(box, caps1, NxVec3(-4,carHeight,-3.5f), NxVec3(0,0,1), false);
CreateRevoluteJoint(box, caps2, NxVec3( 4,carHeight,-3.5f), NxVec3(0,0,1), false);
}
gSelectedActor = caps1;
}
void TriPod::attach(IAgriDevice *device)
{
if(m_attachedTrailer)
return;
if(m_attachedDevice)
return;
m_attachedDevice = device;
Vec3 transformedDeviceConnectorTop;
Vec3 transformedDeviceConnectorBottom;
Vec3 transformedTriPodPos;
Vec3 diff = m_triPodDimms->posTopPodDeviceConnector - m_triPodDimms->posTopPodTractorConnector;
Vec3 transDiff;
D3DXVec3TransformCoord(&transDiff, &diff, &m_matTop);
if(m_frontPod)
{
D3DXVec3TransformCoord(&transformedDeviceConnectorTop, &m_triPodDimms->posTopPodTractorConnector, &m_matTop);
D3DXVec3TransformCoord(&transformedDeviceConnectorBottom, &m_triPodDimms->posBottomPodTractorConnector, &m_matBottom);
}
else
{
D3DXVec3TransformCoord(&transformedDeviceConnectorTop, &m_triPodDimms->posTopPodDeviceConnector, &m_matTop);
D3DXVec3TransformCoord(&transformedDeviceConnectorBottom, &m_triPodDimms->posBottomPodDeviceConnector, &m_matBottom);
}
NxMat34 pose = m_vehicle->getVehicleController()->getActor()->getGlobalPose();
if(m_frontPod)
{
Mat mat;
D3DXMatrixRotationY(&mat, D3DX_PI);
NxMat33 mat33;
NxQuat quat;
pose.M.toQuat(quat);
quat.normalize();
NxReal angle;
NxVec3 axis;
quat.getAngleAxis(angle, axis);
axis.x *= -1;
axis.z *= -1;
//quat.w *= -1;
quat.fromAngleAxis(angle, axis);
pose.M.fromQuat(quat);
pose = NxMat34(NxMat33(NxVec3(mat._11, mat._12, mat._13), NxVec3(mat._21, mat._22, mat._23), NxVec3(mat._31, mat._32, mat._33)), NxVec3(0, 0, 0)) * pose;
}
NxVec3 trans = NxVec3(m_vehicle->getVehicleController()->getForwardVec() * -10);
pose.t.x = pose.t.x + trans.x;
pose.t.y = pose.t.y + trans.y;
pose.t.z = pose.t.z + trans.z;
m_attachedDevice->getActor()->setGlobalPose(pose);
m_attachedDevice->update();
transformedTriPodPos = transformedDeviceConnectorTop - m_triPodDimms->posTopPod;
Vec3 move = transformedTriPodPos - m_attachedDevice->getTriPodPos();
m_attachedDevice->getActor()->setGlobalPosition(m_attachedDevice->getActor()->getGlobalPosition() + NxVec3(move));
m_attachedDevice->update();
//NxRevoluteJointDesc revDescTop;
//revDescTop.actor[0] = m_actorTop;
//revDescTop.actor[1] = m_attachedDevice->getActor();
//revDescTop.localNormal[0] = NxVec3(0, 1, 0);
//revDescTop.localNormal[1] = NxVec3(0, 1, 0);
//revDescTop.setGlobalAxis(NxVec3(0, 0, 1)); //The direction of the axis the bodies revolve around.
//revDescTop.setGlobalAnchor(NxVec3(m_attachedDevice->getTriPodPos() + m_triPodDimms->posTopPod)); //Reference point that the axis passes through.
//m_jointTopPodDevice = (NxRevoluteJoint*)core.dynamics->getScene()->createJoint(revDescTop);
Vec3 leftPos = Vec3(0, 0, m_triPodDimms->width/2);
Mat temp;
m_actorTop->getGlobalPose().getColumnMajor44((NxF32*)&temp);
temp._41 = 0;
temp._42 = 0;
temp._43 = 0;
D3DXVec3TransformCoord(&leftPos, &leftPos, &temp);
//NxRevoluteJointDesc revDescBottomLeft;
//revDescBottomLeft.actor[0] = m_actorBottom;
//revDescBottomLeft.actor[1] = m_attachedDevice->getActor();
//revDescBottomLeft.localNormal[0] = NxVec3(0, 1, 0);
//revDescBottomLeft.localNormal[1] = NxVec3(0, 1, 0);
//revDescBottomLeft.setGlobalAxis(NxVec3(0, 0, 1)); //The direction of the axis the bodies revolve around.
//revDescBottomLeft.setGlobalAnchor(NxVec3(m_attachedDevice->getTriPodPos() + m_triPodDimms->posBottomPod + leftPos)); //Reference point that the axis passes through.
//m_jointBottomLeftPodDevice = (NxRevoluteJoint*)core.dynamics->getScene()->createJoint(revDescBottomLeft);
//NxRevoluteJointDesc revDescBottomRight;
//revDescBottomRight.actor[0] = m_actorBottom;
//revDescBottomRight.actor[1] = m_attachedDevice->getActor();
//revDescBottomRight.localNormal[0] = NxVec3(0, 1, 0);
//revDescBottomRight.localNormal[1] = NxVec3(0, 1, 0);
//revDescBottomRight.setGlobalAxis(NxVec3(0, 0, 1)); //The direction of the axis the bodies revolve around.
//revDescBottomRight.setGlobalAnchor(NxVec3(m_attachedDevice->getTriPodPos() + m_triPodDimms->posBottomPod - leftPos)); //Reference point that the axis passes through.
//m_jointBottomRightPodDevice = (NxRevoluteJoint*)core.dynamics->getScene()->createJoint(revDescBottomRight);
//if(m_f
NxD6JointDesc d6Desc;
d6Desc.actor[0] = m_actorTop;
d6Desc.actor[1] = m_attachedDevice->getActor();
d6Desc.localNormal[0] = NxVec3(0, 1, 0);
d6Desc.localNormal[1] = NxVec3(0, 1, 0);
d6Desc.setGlobalAxis(NxVec3(1, 0, 0)); //The direction of the axis the bodies revolve around.
d6Desc.setGlobalAnchor(NxVec3(m_attachedDevice->getTriPodPos() + m_triPodDimms->posTopPod));
d6Desc.twistMotion = NX_D6JOINT_MOTION_FREE;
d6Desc.swing1Motion = NX_D6JOINT_MOTION_FREE;
d6Desc.swing2Motion = NX_D6JOINT_MOTION_FREE;
d6Desc.xMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc.yMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc.zMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc.projectionMode = NX_JPM_NONE;
d6Desc.projectionMode = NX_JPM_POINT_MINDIST;
d6Desc.projectionDistance = 0.01f;
m_jointTopPodDevice = (NxD6Joint*)core.dynamics->getScene()->createJoint(d6Desc);
NxD6JointDesc d6Desc1;
d6Desc1.actor[0] = m_actorBottom;
d6Desc1.actor[1] = m_attachedDevice->getActor();
d6Desc1.localNormal[0] = NxVec3(0, 1, 0);
d6Desc1.localNormal[1] = NxVec3(0, 1, 0);
d6Desc1.setGlobalAxis(NxVec3(1, 0, 0)); //The direction of the axis the bodies revolve around.
d6Desc1.setGlobalAnchor(NxVec3((m_attachedDevice->getTriPodPos() + m_triPodDimms->posBottomPod) + leftPos));
d6Desc1.twistMotion = NX_D6JOINT_MOTION_FREE;
d6Desc1.swing1Motion = NX_D6JOINT_MOTION_FREE;
d6Desc1.swing2Motion = NX_D6JOINT_MOTION_FREE;
d6Desc1.xMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc1.yMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc1.zMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc1.projectionMode = NX_JPM_NONE;
d6Desc1.projectionMode = NX_JPM_POINT_MINDIST;
d6Desc1.projectionDistance = 0.01f;
m_jointBottomLeftPodDevice = (NxD6Joint*)core.dynamics->getScene()->createJoint(d6Desc1);
NxD6JointDesc d6Desc2;
d6Desc2.actor[0] = m_actorBottom;
d6Desc2.actor[1] = m_attachedDevice->getActor();
d6Desc2.localNormal[0] = NxVec3(0, 1, 0);
d6Desc2.localNormal[1] = NxVec3(0, 1, 0);
d6Desc2.setGlobalAxis(NxVec3(1, 0, 0)); //The direction of the axis the bodies revolve around.
d6Desc2.setGlobalAnchor(NxVec3((m_attachedDevice->getTriPodPos() + m_triPodDimms->posBottomPod) - leftPos));
d6Desc2.twistMotion = NX_D6JOINT_MOTION_FREE;
d6Desc2.swing1Motion = NX_D6JOINT_MOTION_FREE;
d6Desc2.swing2Motion = NX_D6JOINT_MOTION_FREE;
d6Desc2.xMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc2.yMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc2.zMotion = NX_D6JOINT_MOTION_LOCKED;
d6Desc2.projectionMode = NX_JPM_NONE;
d6Desc2.projectionMode = NX_JPM_POINT_MINDIST;
d6Desc2.projectionDistance = 0.01f;
m_jointBottomRightPodDevice = (NxD6Joint*)core.dynamics->getScene()->createJoint(d6Desc2);
return;
}
void Simulation::buildModelRobot(int indexRobot, int indexScene, int indexTeam)
{
//Veiculo descricao
//Body Descricao
NxActor* robotActor = Simulation::getActorRobot(indexScene, indexRobot);
//NxBounds3 bodyBounds;
//robotShapes[0]->getWorldBounds(bodyBounds);
NxVehicleDesc vehicleDesc;
vehicleDesc.position = NxVec3(robotActor->getGlobalPosition());
float mass = 5.;
vehicleDesc.mass = mass;//robotActor->getMass(); //PLUGIN TAH COM PROBLEMA XML ERRADO
//vehicleDesc.motorForce = 70000;
//vehicleDesc.maxVelocity = 300.f;
//vehicleDesc.cameraDistance = 8.0f;
vehicleDesc.centerOfMass.set(NxVec3(0,0,0));//robotActor->getCMassLocalPosition());
//vehicleDesc.differentialRatio = 3.42f;
//vehicleDesc.transmissionEfficiency
vehicleDesc.actor = robotActor;
vehicleDesc.actor->setMaxAngularVelocity(6.2);
vehicleDesc.actor->setMass(mass);
//TODO: LEVANTAR DAMPING
float t = vehicleDesc.actor->getLinearDamping();
float b = vehicleDesc.actor->getAngularDamping();
//vehicleDesc.actor->setAngularDamping(0.5);
//vehicleDesc.actor->setLinearDamping(0.5);
//TODO: LEVANTAR CMASS E INERTIA TENSOR
//vehicleDesc.actor->setCMassOffsetGlobalPosition(NxVec3(0, 0, 0));
NxMat33 inertiaTensor = NxMat33(NxVec3(1294.4362, 3.14502, -66.954), NxVec3(3.14502, 1094.42351, -0.24279), NxVec3(-66.954, -0.24279, 1754.80511));
vehicleDesc.actor->setCMassOffsetLocalPose( NxMat34( inertiaTensor, NxVec3(0,0,0) ) );
//TODO: Diagonalizar inertiaTensor e passar para setMassSpaceInertiaTensor
vehicleDesc.actor->setMassSpaceInertiaTensor(/*vehicleDesc.actor->getMassSpaceInertiaTensor()*1000.*/NxVec3(1764.3, 1284.9, 1094.4) );
//Motor descricao
//NxVehicleMotorDesc motorsDesc[4];
//for(NxU32 i=0;i<4;i++)
//{
//motorsDesc[i].setToCorvette();
//vehicleDesc.motorsDesc.push_back(&motorsDesc[i]);
//}
//Roda (Wheel) descricao
int numberWheels = Simulation::getNumberWheels(indexScene, indexRobot);
NxWheelDesc* wheelDesc = new NxWheelDesc[numberWheels];
for(NxU32 i=0; i<numberWheels; i++)
{
//NxActor* wheelModel = Simulation::getActorWheel(indexScene,indexRobot,i);
//NxActorDesc wheelActorDesc;
//wheelModel->saveToDesc(wheelActorDesc);
//Simulation::gScenes[0]->releaseActor(*wheelModel);
NxActor* actorWheel = Simulation::getActorWheel(indexScene,indexRobot,i);//wheelModel;//Simulation::gScenes[0]->createActor(wheelActorDesc);
//NxShape*const* wheelShapes = actorWheel->getShapes();
//NxBounds3 wheelBounds;
//wheelShapes[0]->getWorldBounds(wheelBounds);
//Para exportar modelo da roda do 3ds Max
// NxWhee
//wheelDesc[i]
//robot1Shapes[0]->isConvexMesh()->getConvexMesh().saveToDesc(convexMesh);
//NxWheelShape* wheelShape = (NxWheelShape*)wheel;
//NxTriangleMeshDesc meshDesc = *((NxTriangleMeshDesc*)(mesh->userData));
//robot1Shapes[0]->isWheel()->
//wheelDesc[i].wheelApproximation = 10;
wheelDesc[i].wheelOrientation = actorWheel->getGlobalOrientation();
wheelDesc[i].position.set(actorWheel->getGlobalPosition()-robotActor->getGlobalPosition());
//wheelDesc[i].position.z = 0;
wheelDesc[i].id = i;
wheelDesc[i].wheelRadius = 27.6;
//wheelDesc[i].wheelWidth = 100;
wheelDesc[i].wheelSuspension = 0;
wheelDesc[i].springRestitution = 0;
wheelDesc[i].springDamping = 0;
wheelDesc[i].springBias = 0.0f;
wheelDesc[i].maxBrakeForce = 100;
wheelDesc[i].frictionToFront = 0.1f;//0.1f; //TODO: CONFIGURAR PARÂMETRO
wheelDesc[i].frictionToSide = 0;//0.02f; //TODO: CONFIGURAR PARÂMETRO
wheelDesc[i].inverseWheelMass = 0.1; //TODO: CONFIGURAR PARÂMETRO
//Angulo das Rodas (Omni)
NxVec3 wheelPosRel = actorWheel->getGlobalPosition() - robotActor->getGlobalPosition();
wheelDesc[i].angWheelRelRobot = NxMath::atan2( wheelPosRel.y, wheelPosRel.x );
vehicleDesc.robotWheels.pushBack(&wheelDesc[i]);
Simulation::gScenes[indexScene]->scene->releaseActor(*actorWheel);
//NxU32 flags = NX_WF_BUILD_LOWER_HALF;
wheelDesc[i].wheelFlags = NX_WF_ACCELERATED | NX_WF_AFFECTED_BY_HANDBRAKE | NX_WF_USE_WHEELSHAPE | NX_WF_BUILD_LOWER_HALF;// |/*NX_WF_STEERABLE_INPUT |*/ flags;
}
//NxBall* teste = Simulation::gScenes[indexScene]->ball;
//Criar robot, vehicle base
NxRobot* robot = (NxRobot*)NxRobot::createVehicle(Simulation::gScenes[indexScene], &vehicleDesc);
if(robot) {
robot->setId(indexRobot);
robot->setIdTeam(indexTeam);
robot->indexScene = indexScene;
//Dribbler and Kicker
for(int i=0; i<robotActor->getNbShapes(); i++) {
NxShape*const* robotShapes = robotActor->getShapes();
const char* shapeName = robotShapes[i]->getName();
if(shapeName) {
char* dribblerName = new char[10];//"Driblador\0"
dribblerName[9] = 0;
memcpy(dribblerName, shapeName, strlen(dribblerName));
char* kickerName = new char[9];//"Chutador\0"
kickerName[8] = 0;
memcpy(kickerName, shapeName, strlen(kickerName));
if(strcmp(dribblerName, "Driblador") == 0) {
robot->dribbler->dribblerShapes.push_back(robotShapes[i]);
}
else if(strcmp(kickerName, "Chutador") == 0) {
robot->kicker->kickerShapeDesc = Simulation::copyShapeDesc(robotShapes[i]);
robotActor->releaseShape(*(robotShapes[i]));
}
delete dribblerName;
delete kickerName;
}
}
//Initial Pose
robot->setInitialPose(robotActor->getGlobalPose());
robotActor->putToSleep();
//Mudar pose do robo
//NxQuat q;
//q.
//q.fromAngleAxis(180.0f, NxVec3(0.0f, 1.0f, 0.0f));
//robot->getActor()->setGlobalPose(pose);
//Release no actor importado do 3ds Max
//gScenes[0]->releaseActor(*robotActor);
string label;
string plabel = "Robo";
stringstream out;
out << indexRobot;
out << "-";
out << indexTeam;
//out << "-";
//out << indexScene;
label.append(plabel);
label.append(out.str());
char* arrayLabel = new char[label.size()+1];
arrayLabel[label.size()]=0;
memcpy(arrayLabel, label.c_str(), label.size());
robotActor->setName(arrayLabel);
//delete arrayLabel;
}
}