bool PhysXCharacterController::filter(const PxController& a, const PxController& b)
{
CharacterController* controllerA = (CharacterController*)a.getUserData();
CharacterController* controllerB = (CharacterController*)b.getUserData();
bool canCollide = gPhysics().isCollisionEnabled(controllerA->getLayer(), controllerB->getLayer());
return canCollide;
}
/// Let's check if actors are okay
void PhysicsSystemPhysX::syncActors()
{
for (std::size_t i = 0; i < mWorld->actors.size(); ++i)
{
Actor* actor = mWorld->actors[i];
for (std::size_t j = 0; j < actor->components.size(); ++j)
{
ACharacterComponent* chr = dynamic_cast<ACharacterComponent*>(actor->components[j]);
if (chr)
{
// We have a character controller in this actor, needs to be handled
if (chr->userData)
{
PxController* cc = (PxController*)chr->userData;
chr->t.position.x = cc->getPosition().x;
chr->t.position.y = cc->getPosition().y;
chr->t.position.z = cc->getPosition().z;
//Log("Updated actor");
}
else
{
PxMaterial* mat = mPhysics->createMaterial(0.1, 0.1, 0.1);
PxCapsuleControllerDesc* desc = new PxCapsuleControllerDesc;
desc->height = 5;
desc->radius = 5;
desc->position = PxExtendedVec3(0, 0, 0);
desc->material = mat;
Log("BOX VALID: %d", desc->isValid());
PxController* c = manager->createController(*desc);
if (!c)
{
Log("Failed to instance a controller");
}
if (c)
{
chr->userData = c;
chr->moving.connect(sigc::mem_fun(this, &PhysicsSystemPhysX::TestCharacterMove));
Log("PhysX validated an actor's character controller");
}
}
}
}
}
}
void PhysicsSystemPhysX::TestCharacterMove(Vector3D displacement, Actor* a)
{
ACharacterComponent* chr = dynamic_cast<ACharacterComponent*>(a->getRootComponent());
if (chr)
{
if (chr->userData)
{
PxController* cc = (PxController*)chr->userData;
PxControllerFilters filters;
cc->move(PxVec3(displacement.x, displacement.y, displacement.z), 0.01f, 1.f / 60.f, filters);
//Log("Moving..");
}
}
}
void CharacterControllerManager::releaseController(PxController& controller)
{
for (PxU32 i = 0; i<mControllers->size(); i++)
if ((*mControllers)[i]->getNxController() == &controller)
{
mControllers->replaceWithLast(i);
break;
}
if (controller.getType() == PxControllerShapeType::eCAPSULE)
{
CapsuleController* cc = (CapsuleController*)&controller;
cc->~CapsuleController();
mAllocator->deallocate(cc);
}
else if (controller.getType() == PxControllerShapeType::eBOX)
{
BoxController* bc = (BoxController*)&controller;
bc->~BoxController();
mAllocator->deallocate(bc);
}
else PX_ASSERT(0);
}
void CharacterControllerManager::releaseController(PxController& controller)
{
for(PxU32 i = 0; i<mControllers.size(); i++)
{
if(mControllers[i]->getPxController() == &controller)
{
mControllers.replaceWithLast(i);
break;
}
}
if(controller.getType() == PxControllerShapeType::eCAPSULE)
{
CapsuleController* cc = (CapsuleController*)&controller;
PX_DELETE(cc);
}
else if(controller.getType() == PxControllerShapeType::eBOX)
{
BoxController* bc = (BoxController*)&controller;
PX_DELETE(bc);
}
else PX_ASSERT(0);
}
void *timer_handler(void *ptr) {
if(timer_disable == 1) {
return NULL;
}
struct timespec _t;
static struct timeval now, prev;
double dt = 0;
clock_gettime(CLOCK_REALTIME, &_t);
static PxController ctrlr;
// SioClient initialization -------------------------------------------------
SioClientWrapper client;
sio::message::ptr data;
//発生するイベント名一覧をstd::vector<std::string>としてclientに渡す
std::vector<std::string> eventList(0);
eventList.push_back("landing");
eventList.push_back("direction");
eventList.push_back("px_bounce");
eventList.push_back("px_start");
eventList.push_back("px_position");
eventList.push_back("px_velocity");
client.setEventList(eventList);
//自身を表す部屋名を設定する(Phenoxなら例えば"Phenox"と決める)
client.setMyRoom("Phenox");
//データの送信宛先となる部屋名を設定する(Gameサーバなら例えば"Game")
client.setDstRoom("Game");
//URLを指定して接続開始
client.start("http://localhost:8000");
while(1) {
pxset_keepalive();
pxset_systemlog();
pxset_img_seq(cameraid);
static unsigned long msec_cnt = 0;
msec_cnt++;
gettimeofday(&now, NULL);
dt = (now.tv_sec - prev.tv_sec) +
(now.tv_usec - prev.tv_usec) * 1.0E-6;
if(dt < 0){
cout << "dt < 0" << endl;
continue;
}
prev = now;
// vision control
static px_selfstate st;
static Vector2f pos;
pxget_selfstate(&st);
pos << st.vision_tx, st.vision_ty;
Vector2f norm, norm_start;
Vector2f norm2, norm_start2;
static Vector2f input(0,0);
// -------------------------------------------------------------------
// get boundary norm -------------------------------------------------
// -------------------------------------------------------------------
int boundary_cnt = 0;
if(pthread_mutex_trylock(&mutex) != EBUSY){
norm = gnorm;
norm_start = gnorm_start;
norm2 = gnorm2;
norm_start2 = gnorm_start2;
boundary_cnt = gboundary_cnt;
//cout << "boundary cnt = " << boundary_cnt << endl;
//cout << "norm = \n" << norm << endl;
//cout << "norm2 = \n" << norm2 << endl;
pthread_mutex_unlock(&mutex);
ctrlr.boundHandler(boundary_cnt,norm,norm2,pos);
}
// --------------------------------------------------------------------
// get landing and direction-------------------------------------------
// --------------------------------------------------------------------
//"landing"に対応するデータが来ているかチェック
if (client.isUpdated("landing")){
data = client.getData("landing");//データをsio::message::ptrとして取得
parseLanding(data);//データ抽出用関数に渡す
std::cout << "landing=" << landing << std::endl;
}
//"direction"に対応するデータが来ているかチェック
if (client.isUpdated("direction")){
data = client.getData("direction");//データをsio::message::ptrとして取得
parseDirection(data);//データ抽出用関数に渡す
std::cout << "direction = [" << direction[0] << ", " << direction[1] << "]" << std::endl;
}
// --------------------------------------------------------------------
// Sample of data sending ---------------------------------------------
// --------------------------------------------------------------------
// 送りたいところに移動してね
Vector2f px_position(0, 0);
Vector2f px_velocity(0, 0);
client.sendData("px_start", makePxStart());//
client.sendData("px_bounce", makePxBounce());//
client.sendData("px_position", makePxPosition());//
client.sendData("px_velocity", makePxVelocity());//
//cout << ctrlr.vx() << "," << ctrlr.vy() << endl;
// save log
static ofstream ofs_deg("output_deg");
ofs_deg << st.degx << "," << st.degy << endl;
static ofstream ofs_ctl("output_v");
ofs_ctl << ctrlr.vx() << "," << ctrlr.vy() << "," << norm.x() << "," << norm.y() << endl;
static ofstream ofs_vision("output_vision");
ofs_vision << st.vision_tx << "," << st.vision_ty << "," << input.x() << "," << input.y() << endl;
// if(!(msec_cnt % 30)){
// printf("%.2f %.2f %.2f | %.2f %.2f %.2f | %.2f | \n",st.degx,st.degy,st.degz,st.vision_tx,st.vision_ty,st.vision_tz,st.height);
// }
static int hover_cnt = 0;
static Vector2f origin(0,0);
if(pxget_operate_mode() == PX_UP){
pxset_visualselfposition(0, 0);
hover_cnt = 0;
}
if(pxget_operate_mode() == PX_HOVER) {
if(hover_cnt < 500){
hover_cnt++;
}
else if (hover_cnt == 500) {
cout << "start control" << endl;
ctrlr.init(0,1,origin,pos);
hover_cnt++;
}
else{
input = ctrlr.controlStep(pos, dt);
pxset_visioncontrol_xy(input.x(),input.y());
}
}
static int prev_operatemode = PX_HALT;
if((prev_operatemode == PX_UP) && (pxget_operate_mode() == PX_HOVER)) {
origin << st.vision_tx, st.vision_ty;
pxset_visioncontrol_xy(origin.x(),origin.y());
}
prev_operatemode = pxget_operate_mode();
if(pxget_whisle_detect() == 1) {
if(pxget_operate_mode() == PX_HOVER) {
pxset_operate_mode(PX_DOWN);
}
else if(pxget_operate_mode() == PX_HALT) {
pxset_rangecontrol_z(150);
pxset_operate_mode(PX_UP);
}
}
if(pxget_battery() == 1) {
timer_disable = 1;
system("shutdown -h now\n");
exit(1);
}
struct timespec remains;
_t.tv_nsec += 10000000;
clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &_t, &remains);
// while (get_time() - t < 10.0) {
// usleep(500);
// }
clock_gettime(CLOCK_REALTIME, &_t);
}
}
PxController* ControlledActor::init(const ControlledActorDesc& desc, PxControllerManager* manager)
{
const float radius = desc.mRadius;
float height = desc.mHeight;
float crouchHeight = desc.mCrouchHeight;
PxControllerDesc* cDesc;
PxBoxControllerDesc boxDesc;
PxCapsuleControllerDesc capsuleDesc;
if(desc.mType==PxControllerShapeType::eBOX)
{
height *= 0.5f;
height += radius;
crouchHeight *= 0.5f;
crouchHeight += radius;
boxDesc.halfHeight = height;
boxDesc.halfSideExtent = radius;
boxDesc.halfForwardExtent = radius;
cDesc = &boxDesc;
}
else
{
PX_ASSERT(desc.mType==PxControllerShapeType::eCAPSULE);
capsuleDesc.height = height;
capsuleDesc.radius = radius;
capsuleDesc.climbingMode = PxCapsuleClimbingMode::eCONSTRAINED;
cDesc = &capsuleDesc;
}
cDesc->density = desc.mProxyDensity;
cDesc->scaleCoeff = desc.mProxyScale;
cDesc->material = &mOwner.getDefaultMaterial();
cDesc->position = desc.mPosition;
cDesc->slopeLimit = desc.mSlopeLimit;
cDesc->contactOffset = desc.mContactOffset;
cDesc->stepOffset = desc.mStepOffset;
cDesc->invisibleWallHeight = desc.mInvisibleWallHeight;
cDesc->maxJumpHeight = desc.mMaxJumpHeight;
// cDesc->nonWalkableMode = PxControllerNonWalkableMode::ePREVENT_CLIMBING_AND_FORCE_SLIDING;
cDesc->reportCallback = desc.mReportCallback;
cDesc->behaviorCallback = desc.mBehaviorCallback;
cDesc->volumeGrowth = desc.mVolumeGrowth;
mType = desc.mType;
mInitialPosition = desc.mPosition;
mStandingSize = height;
mCrouchingSize = crouchHeight;
mControllerRadius = radius;
PxController* ctrl = static_cast<PxBoxController*>(manager->createController(*cDesc));
PX_ASSERT(ctrl);
// remove controller shape from scene query for standup overlap test
PxRigidDynamic* actor = ctrl->getActor();
if(actor)
{
if(actor->getNbShapes())
{
PxShape* ctrlShape;
actor->getShapes(&ctrlShape,1);
ctrlShape->setFlag(PxShapeFlag::eSCENE_QUERY_SHAPE, false);
Renderer* renderer = mOwner.getRenderer();
if(desc.mType==PxControllerShapeType::eBOX)
{
const PxVec3 standingExtents(radius, height, radius);
const PxVec3 crouchingExtents(radius, crouchHeight, radius);
mRenderActorStanding = SAMPLE_NEW(RenderBoxActor)(*renderer, standingExtents);
mRenderActorCrouching = SAMPLE_NEW(RenderBoxActor)(*renderer, crouchingExtents);
}
else if(desc.mType==PxControllerShapeType::eCAPSULE)
{
mRenderActorStanding = SAMPLE_NEW(RenderCapsuleActor)(*renderer, radius, height*0.5f);
mRenderActorCrouching = SAMPLE_NEW(RenderCapsuleActor)(*renderer, radius, crouchHeight*0.5f);
}
}
}
mController = ctrl;
return ctrl;
}