void Enemy::CheckFutureCollision(int attempts)
{
attempts++;
if(attempts > 10) return;
PxVec3 pos = actor->getGlobalPose().p;
PxVec3 dirMove = PxVec3(moveDir.x,0,moveDir.z);
while(dirMove.isZero())
{
float xspeed = rand()% (int)ceil(movementSpeed); //getal van 0 tot 10
float xfSpeed = xspeed - (int)ceil(movementSpeed)/2; // getal van -5 tot 5
float zspeed = rand()%(int)ceil(movementSpeed); //getal van 0 tot 10
float zfSpeed = zspeed -(int)ceil( movementSpeed)/2; // getal van -5 tot 5
dirMove = PxVec3(xfSpeed, 0, zfSpeed);
}
dirMove.normalize();
int numHits;
PxRaycastHit* hit = physics->RaycastMultiple(pos-PxVec3(0,1.5f,0),dirMove,3.5f,&numHits,PxQueryFlag::eSTATIC | PxQueryFlag::eDYNAMIC);
for(unsigned int i= 0; i < numHits; i++)
{
if(hit[i].actor != actor && hit[i].actor != NULL)
{
currentMoveTime = 0;
float xspeed = rand()% (int)ceil(movementSpeed); //number from 0 to movementSpeed
float xfSpeed = xspeed - (int)ceil(movementSpeed)/2; // number from -movementSpeed/2 to movementSpeed/2
float zspeed = rand()%(int)ceil(movementSpeed);
float zfSpeed = zspeed -(int)ceil( movementSpeed)/2;
moveDir = D3DXVECTOR3(xfSpeed, 0, zfSpeed);
i = 999;
CheckFutureCollision(attempts);
}
}
}
void NpArticulationLink::setAngularVelocity(const PxVec3& velocity, bool autowake)
{
NpScene* scene = NpActor::getOwnerScene(*this);
PX_CHECK_AND_RETURN(velocity.isFinite(), "NpArticulationLink::setAngularVelocity velocity is not valid.");
NP_WRITE_CHECK(scene);
getScbBodyFast().setAngularVelocity(velocity);
if (scene)
mRoot->wakeUpInternal((!velocity.isZero()), autowake);
}
void NpArticulationLink::addTorque(const PxVec3& torque, PxForceMode::Enum mode, bool autowake)
{
NpScene* scene = NpActor::getOwnerScene(*this);
PX_UNUSED(scene);
PX_CHECK_AND_RETURN(torque.isFinite(), "NpArticulationLink::addTorque: force is not valid.");
NP_WRITE_CHECK(scene);
PX_CHECK_AND_RETURN(scene, "NpArticulationLink::addTorque: articulation link must be in a scene!");
addSpatialForce(0, &torque, mode);
mRoot->wakeUpInternal((!torque.isZero()), autowake);
}
void NpRigidDynamic::addTorque(const PxVec3& torque, PxForceMode::Enum mode, bool autowake)
{
Scb::Body& b = getScbBodyFast();
PX_CHECK_AND_RETURN(torque.isFinite(), "NpRigidDynamic::addTorque: torque is not valid.");
NP_WRITE_CHECK(NpActor::getOwnerScene(*this));
PX_CHECK_AND_RETURN(NpActor::getAPIScene(*this), "RigidDynamic::addTorque: Body must be in a scene!");
PX_CHECK_AND_RETURN(!(b.getFlags() & PxRigidBodyFlag::eKINEMATIC), "RigidDynamic::addTorque: Body must be non-kinematic!");
PX_CHECK_AND_RETURN(!(b.getActorFlags() & PxActorFlag::eDISABLE_SIMULATION), "RigidDynamic::addTorque: Not allowed if PxActorFlag::eDISABLE_SIMULATION is set!");
addSpatialForce(0, &torque, mode);
wakeUpInternalNoKinematicTest(b, (!torque.isZero()), autowake);
}
void NpRigidDynamic::setAngularVelocity(const PxVec3& velocity, bool autowake)
{
NpScene* scene = NpActor::getAPIScene(*this);
NP_WRITE_CHECK(NpActor::getOwnerScene(*this));
PX_CHECK_AND_RETURN(velocity.isFinite(), "NpRigidDynamic::setAngularVelocity: velocity is not valid.");
PX_CHECK_AND_RETURN(!(getScbBodyFast().getFlags() & PxRigidBodyFlag::eKINEMATIC), "RigidDynamic::setAngularVelocity: Body must be non-kinematic!");
PX_CHECK_AND_RETURN(!(getScbBodyFast().getActorFlags() & PxActorFlag::eDISABLE_SIMULATION), "RigidDynamic::setAngularVelocity: Not allowed if PxActorFlag::eDISABLE_SIMULATION is set!");
Scb::Body& b = getScbBodyFast();
b.setAngularVelocity(velocity);
if (scene)
wakeUpInternalNoKinematicTest(b, (!velocity.isZero()), autowake);
}
void Gu::TriangleMesh::debugVisualize(
Cm::RenderOutput& out, const PxTransform& pose, const PxMeshScale& scaling, const PxBounds3& cullbox,
const PxU64 mask, const PxReal fscale, const PxU32 numMaterials) const
{
PX_UNUSED(numMaterials);
//bool cscale = !!(mask & ((PxU64)1 << PxVisualizationParameter::eCULL_BOX));
const PxU64 cullBoxMask = PxU64(1) << PxVisualizationParameter::eCULL_BOX;
bool cscale = ((mask & cullBoxMask) == cullBoxMask);
const PxMat44 midt(PxIdentity);
const Cm::Matrix34 absPose(PxMat33(pose.q) * scaling.toMat33(), pose.p);
PxU32 nbTriangles = getNbTrianglesFast();
const PxU32 nbVertices = getNbVerticesFast();
const PxVec3* vertices = getVerticesFast();
const void* indices = getTrianglesFast();
const PxDebugColor::Enum colors[] =
{
PxDebugColor::eARGB_BLACK,
PxDebugColor::eARGB_RED,
PxDebugColor::eARGB_GREEN,
PxDebugColor::eARGB_BLUE,
PxDebugColor::eARGB_YELLOW,
PxDebugColor::eARGB_MAGENTA,
PxDebugColor::eARGB_CYAN,
PxDebugColor::eARGB_WHITE,
PxDebugColor::eARGB_GREY,
PxDebugColor::eARGB_DARKRED,
PxDebugColor::eARGB_DARKGREEN,
PxDebugColor::eARGB_DARKBLUE,
};
const PxU32 colorCount = sizeof(colors)/sizeof(PxDebugColor::Enum);
if(cscale)
{
const Gu::Box worldBox(
(cullbox.maximum + cullbox.minimum)*0.5f,
(cullbox.maximum - cullbox.minimum)*0.5f,
PxMat33(PxIdentity));
// PT: TODO: use the callback version here to avoid allocating this huge array
PxU32* results = reinterpret_cast<PxU32*>(PX_ALLOC_TEMP(sizeof(PxU32)*nbTriangles, "tmp triangle indices"));
LimitedResults limitedResults(results, nbTriangles, 0);
Midphase::intersectBoxVsMesh(worldBox, *this, pose, scaling, &limitedResults);
nbTriangles = limitedResults.mNbResults;
if (fscale)
{
const PxU32 fcolor = PxU32(PxDebugColor::eARGB_DARKRED);
for (PxU32 i=0; i<nbTriangles; i++)
{
const PxU32 index = results[i];
PxVec3 wp[3];
getTriangle(*this, index, wp, vertices, indices, absPose, has16BitIndices());
const PxVec3 center = (wp[0] + wp[1] + wp[2]) / 3.0f;
PxVec3 normal = (wp[0] - wp[1]).cross(wp[0] - wp[2]);
PX_ASSERT(!normal.isZero());
normal = normal.getNormalized();
out << midt << fcolor <<
Cm::DebugArrow(center, normal * fscale);
}
}
if (mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_SHAPES))
{
const PxU32 scolor = PxU32(PxDebugColor::eARGB_MAGENTA);
out << midt << scolor; // PT: no need to output this for each segment!
PxDebugLine* segments = out.reserveSegments(nbTriangles*3);
for(PxU32 i=0; i<nbTriangles; i++)
{
const PxU32 index = results[i];
PxVec3 wp[3];
getTriangle(*this, index, wp, vertices, indices, absPose, has16BitIndices());
segments[0] = PxDebugLine(wp[0], wp[1], scolor);
segments[1] = PxDebugLine(wp[1], wp[2], scolor);
segments[2] = PxDebugLine(wp[2], wp[0], scolor);
segments+=3;
}
}
if ((mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_EDGES)) && mExtraTrigData)
visualizeActiveEdges(out, *this, nbTriangles, results, absPose, midt);
PX_FREE(results);
}
else
{
if (fscale)
{
const PxU32 fcolor = PxU32(PxDebugColor::eARGB_DARKRED);
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, vertices, indices, absPose, has16BitIndices());
const PxVec3 center = (wp[0] + wp[1] + wp[2]) / 3.0f;
PxVec3 normal = (wp[0] - wp[1]).cross(wp[0] - wp[2]);
PX_ASSERT(!normal.isZero());
normal = normal.getNormalized();
out << midt << fcolor <<
Cm::DebugArrow(center, normal * fscale);
}
}
if (mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_SHAPES))
{
PxU32 scolor = PxU32(PxDebugColor::eARGB_MAGENTA);
out << midt << scolor; // PT: no need to output this for each segment!
PxVec3* transformed = reinterpret_cast<PxVec3*>(PX_ALLOC(sizeof(PxVec3)*nbVertices, "PxVec3"));
for(PxU32 i=0;i<nbVertices;i++)
transformed[i] = absPose.transform(vertices[i]);
PxDebugLine* segments = out.reserveSegments(nbTriangles*3);
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, transformed, indices, has16BitIndices());
const PxU32 localMaterialIndex = getTriangleMaterialIndex(i);
scolor = colors[localMaterialIndex % colorCount];
segments[0] = PxDebugLine(wp[0], wp[1], scolor);
segments[1] = PxDebugLine(wp[1], wp[2], scolor);
segments[2] = PxDebugLine(wp[2], wp[0], scolor);
segments+=3;
}
PX_FREE(transformed);
}
if ((mask & (PxU64(1) << PxVisualizationParameter::eCOLLISION_EDGES)) && mExtraTrigData)
visualizeActiveEdges(out, *this, nbTriangles, NULL, absPose, midt);
}
}
void Gu::TriangleMesh::debugVisualize(
Cm::RenderOutput& out, const Cm::Matrix34& absPose, const PxBounds3& cullbox,
const PxU64 mask, const PxReal fscale) const
{
bool cscale = !!(mask & ((PxU64)1 << PxVisualizationParameter::eCULL_BOX));
const PxMat44 midt = PxMat44::createIdentity();
const PxU32 nbTriangles = mesh.getNumTriangles();
const PxU32 nbVertices = mesh.getNumVertices();
const PxVec3* vertices = mesh.getVertices();
const void* indices = getTrianglesFast();
const bool has16BitIndices = mesh.has16BitIndices();
if (fscale)
{
const PxU32 fcolor = PxDebugColor::eARGB_DARKRED;
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, vertices, indices, absPose, has16BitIndices);
const PxVec3 center = (wp[0] + wp[1] + wp[2]) / 3.0f;
PxVec3 normal = (wp[0] - wp[1]).cross(wp[0] - wp[2]);
PX_ASSERT(!normal.isZero());
normal = normal.getNormalized();
if (!cscale || cullbox.contains(center))
out << midt << fcolor <<
Cm::DebugArrow(center, normal * fscale);
}
}
if (mask & ((PxU64)1 << PxVisualizationParameter::eCOLLISION_SHAPES))
{
const PxU32 scolor = PxDebugColor::eARGB_MAGENTA;
out << midt << scolor; // PT: no need to output this for each segment!
// PT: transform vertices only once
PxVec3* transformed = (PxVec3*)PX_ALLOC(sizeof(PxVec3)*nbVertices);
for(PxU32 i=0;i<nbVertices;i++)
transformed[i] = absPose.transform(vertices[i]);
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, transformed, indices, has16BitIndices);
if (!cscale || (cullbox.contains(wp[0]) && cullbox.contains(wp[1]) && cullbox.contains(wp[2])))
{
out.outputSegment(wp[0], wp[1]);
out.outputSegment(wp[1], wp[2]);
out.outputSegment(wp[2], wp[0]);
}
}
PX_FREE(transformed);
}
if (mask & ((PxU64)1 << PxVisualizationParameter::eCOLLISION_EDGES))
{
const PxU32 ecolor = PxDebugColor::eARGB_YELLOW;
for (PxU32 i=0; i<nbTriangles; i++)
{
PxVec3 wp[3];
getTriangle(*this, i, wp, vertices, indices, absPose, has16BitIndices);
const PxU32 flags = mesh.getTrigSharedEdgeFlags(i);
if(flags & Gu::ETD_CONVEX_EDGE_01)
{
if (!cscale || (cullbox.contains(wp[0]) && cullbox.contains(wp[1])))
out << midt << ecolor << Cm::RenderOutput::LINES << wp[0] << wp[1];
}
if(flags & Gu::ETD_CONVEX_EDGE_12)
{
if (!cscale || (cullbox.contains(wp[1]) && cullbox.contains(wp[2])))
out << midt << ecolor << Cm::RenderOutput::LINES << wp[1] << wp[2];
}
if(flags & Gu::ETD_CONVEX_EDGE_20)
{
if (!cscale || (cullbox.contains(wp[0]) && cullbox.contains(wp[2])))
out << midt << ecolor << Cm::RenderOutput::LINES << wp[0] << wp[2];
}
}
}
}
/**
@brief
@date 2013-12-19
*/
void CCreature::CreateJoint( CPhenotypeNode *parentNode, CPhenotypeNode *childNode, genotype_parser::SConnection *connect,
const PxVec3 &conPos )
{
RET(!childNode);
RET(!parentNode);
PxRigidDynamic* body = parentNode->m_pBody;
PxRigidDynamic *child = childNode->m_pBody;
genotype_parser::SConnection *joint = connect;
//PxVec3 dir0(joint->parentOrient.dir.x, joint->parentOrient.dir.y, joint->parentOrient.dir.z);
//PxVec3 dir1(joint->orient.dir.x, joint->orient.dir.y, joint->orient.dir.z);
//PxVec3 pos = conPos;
//// random position
//PxVec3 randPos(connect->randPos.x, connect->randPos.y, connect->randPos.z);
//pos += RandVec3(randPos, 1.f);
//// random orientation
//PxVec3 randOrient(connect->randOrient.x, connect->randOrient.y, connect->randOrient.z);
//if (!dir1.isZero())
//{
// dir1 += RandVec3(randOrient, 1.f);
// dir1.normalize();
//}
//PxTransform tm0 = (dir0.isZero())? PxTransform::createIdentity() : PxTransform(PxQuat(joint->parentOrient.angle, dir0));
//PxTransform tm1 = (dir1.isZero())? PxTransform(PxVec3(pos)) :
// (PxTransform(PxQuat(joint->orient.angle, dir1)) * PxTransform(PxVec3(pos)));
PxTransform tm0, tm1;
GetJointTransform(&conPos, joint, tm0, tm1);
PxVec3 limit = utility::Vec3toPxVec3(joint->limit);
PxVec3 velocity = utility::Vec3toPxVec3(joint->velocity);
//apply gravity direction only have root genotype
if (boost::iequals(parentNode->m_ShapeName, "root"))
{
PxVec3 gravDir = parentNode->m_pBody->getGlobalPose().p;
gravDir.normalize();
gravDir = PxVec3(0,1,0);
PxQuat gravQ;
utility::quatRotationArc(gravQ, PxVec3(0,1,0), gravDir);
tm1 = tm1 * PxTransform(gravQ);
}
PxJoint* pxJoint = NULL;
const PxReal tolerance = 0.2f;
if (boost::iequals(joint->type, "fixed"))
{
PxFixedJoint *j = PxFixedJointCreate(m_sample.getPhysics(), body, tm0, child, tm1);
j->setProjectionLinearTolerance(tolerance);
j->setConstraintFlag(PxConstraintFlag::ePROJECTION, true);
pxJoint = j;
}
else if (boost::iequals(joint->type, "spherical"))
{
if (PxSphericalJoint *j = PxSphericalJointCreate(m_sample.getPhysics(), body, tm0, child, tm1) )
{
if (!limit.isZero())
{
j->setLimitCone(PxJointLimitCone(limit.x, limit.y, PxSpring(0,0)));
j->setSphericalJointFlag(PxSphericalJointFlag::eLIMIT_ENABLED, true);
}
j->setProjectionLinearTolerance(tolerance);
j->setConstraintFlag(PxConstraintFlag::ePROJECTION, true);
pxJoint = j;
}
}
else if (boost::iequals(joint->type, "revolute"))
{
if (PxRevoluteJoint*j = PxRevoluteJointCreate(m_sample.getPhysics(), body, tm0, child, tm1) )
{
if (!limit.isZero())
{
j->setLimit(PxJointAngularLimitPair(limit.x, limit.y, limit.z)); // upper, lower, tolerance
j->setRevoluteJointFlag(PxRevoluteJointFlag::eLIMIT_ENABLED, true);
}
if (!velocity.isZero())
{
j->setDriveVelocity(velocity.x);
j->setRevoluteJointFlag(PxRevoluteJointFlag::eDRIVE_ENABLED, true);
}
j->setProjectionLinearTolerance(tolerance);
j->setConstraintFlag(PxConstraintFlag::ePROJECTION, true);
pxJoint = j;
}
}
CJoint *pJoint = new CJoint(parentNode, childNode, tm0, tm1, pxJoint, velocity.x, joint->period);
CAngularSensor *pSensor = new CAngularSensor();
CMuscleEffector *pEffector = new CMuscleEffector(joint->period);
pJoint->ApplySensor(*pSensor);
pJoint->ApplyEffector(*pEffector);
parentNode->m_Joints.push_back(pJoint);
parentNode->m_Sensors.push_back(pSensor);
parentNode->m_Effectors.push_back(pEffector);
{ // add child
CAngularSensor *pChildSensor = new CAngularSensor();
pJoint->ApplySensor(*pChildSensor);
childNode->m_pParentJointSensor = pChildSensor;
}
}
