[Solution] Jump with PlayerController

Ok since I'm seeing a lot of people in trouble to put PlayerController working with jump, I'm pasting here my code.

Function that detects ground collision:

bool cEntityCollisionHandler::groundCollision(OgreNewt::Body* body, Ogre::Real bodyHeight)
{
Ogre::Vector3 pos;

pos = Ogre::Vector3(cPhysicsInterface::getSingleton().getPosition(body).x,cPhysicsInterface::getSingleton().getPosition(body).y - (bodyHeight / 2),cPhysicsInterface::getSingleton().getPosition(body).z);
OgreNewt::BasicRaycast camRay(cPhysicsInterface::getSingleton().getNewtWorld(),Ogre::Vector3(pos.x,pos.y,pos.z),Ogre::Vector3(pos.x,pos.y - 1.0f,pos.z),false);

/* get collision distance it's comment because you don't realy need it for this propose but can be useful to do other things
Ogre::Vector3 direction = pos - (pos - Vector3(0,-1.0f,0));
Ogre::Real distance = direction.length();
distance = distance * camRay.getFirstHit().mDistance;
*/

if (camRay.getHitCount() > 0)
{
return true;
}
return false;
}


PlayerFrameListener code:

#include "cPlayerFrameListener.h"
#include "cPlayerCollisionHandler.h"
#include "Debugger.h"
#include "cPhysicsInterface.h"


cPlayerFrameListener::cPlayerFrameListener(OgreNewt::PlayerController* playerController, cPlayerCollisionHandler *entityCollision, cPlayer *player, bool bufferedKeys , bool bufferedMouse, bool bufferedJoy):cBasicFrameListener(true,true,false)
{

cameraPitchAngle = 0.0f;

// set the external view distance to 3 meters
m_cameraMode = 1;
m_cameraKeyState = false;
m_extenalViewDist = 5.0f;

mPlayerController = playerController;
mPlayer = player;
mEntityCollision = entityCollision;

// Set desired framerate and variables used for stepping the physics
desiredPhysicsFramerate = 120;
lastFPS = 0;
lastFrameDuration = 0;
physicsUpdateStep = 1.0f / static_cast<float>(desiredPhysicsFramerate);
physicsTimeAccumulator = 0.0f;

OgreNewt::Debugger &debug(cPhysicsInterface::getSingleton().getNewtWorld()->getDebugger());
debug.init(cGraphicalInterface::getSingleton().getSceneManager());
}



bool cPlayerFrameListener::mouseMoved( const OIS::MouseEvent &arg )
{
return true;
}

bool cPlayerFrameListener::mousePressed( const OIS::MouseEvent &arg, OIS::MouseButtonID id )
{
return true;
}

bool cPlayerFrameListener::mouseReleased( const OIS::MouseEvent &arg, OIS::MouseButtonID id )
{
return true;
}


void cPlayerFrameListener::input(const FrameEvent& evt)
{
mKeyboard->capture();
mMouse->capture();

// Get frame rendertime in milliseconds
unsigned long currentFrameTime = cGraphicalInterface::getSingleton().getRoot()->getTimer()->getMilliseconds();
lastFrameDuration = currentFrameTime - lastFrameTime;
lastFrameTime = currentFrameTime;
bool isOnTheGround = false;

// Add last frame duration to accumulator that uses seconds
physicsTimeAccumulator += lastFrameDuration / 1000.0f;

// Check whether there is any reason to step the physics
if (physicsTimeAccumulator > physicsUpdateStep)
{
// Lets make maximum of 5 physics updates
if (physicsTimeAccumulator < physicsUpdateStep * 5)
{
while (physicsTimeAccumulator >= physicsUpdateStep)
{
// Update the physics world and substract from the accumulator
cPhysicsInterface::getSingleton().getNewtWorld()->update(physicsUpdateStep);
physicsTimeAccumulator -= physicsUpdateStep;
}
}
else
{
// Too many steps, just update once and clear accumulator to prevent total slowdown
cPhysicsInterface::getSingleton().getNewtWorld()->update(physicsTimeAccumulator);
physicsTimeAccumulator = 0.0f;
}
}

// player movement
Ogre::Real forwardSpeed, sideSpeed;
Ogre::Radian heading;

mPlayerController->getVelocity(forwardSpeed, sideSpeed, heading);
forwardSpeed = 0;
sideSpeed = 0;
OgreNewt::Body* playerBody = mPlayerController->getBody0();

//jumping
isOnTheGround = mEntityCollision->groundCollision(playerBody,mPlayerController->getPlayerHeight());

if(isOnTheGround)
mPlayer->setIsJumping(false);
else
mPlayer->setIsJumping(true);

//running
if(mPlayer->getIsRunning() == false) //Restores stamina
{
mPlayer->setStamina(mPlayer->getStamina()+ evt.timeSinceLastFrame);
if(mPlayer->getStamina() >= mPlayer->getStaminaLimit())
{
mPlayer->setStamina(mPlayer->getStaminaLimit());
}
}

// set player velocity from the user
if(mKeyboard->isKeyDown(OIS::KC_W))
{
if(mPlayer->getIsRunning() == false)
{
mPlayer->setRunVel(Ogre::Vector3(1,1,1));
}
forwardSpeed += (50.0f * mPlayer->getRunVel().x);
}

if(mKeyboard->isKeyDown(OIS::KC_S))
{
if(mPlayer->getIsRunning() == false)
{
mPlayer->setRunVel(Ogre::Vector3(1,1,1));
}
forwardSpeed -= (30.0f * mPlayer->getRunVel().x);
}
if(mKeyboard->isKeyDown(OIS::KC_A))
{
if(mPlayer->getIsRunning() == false)
{
mPlayer->setRunVel(Ogre::Vector3(1,1,1));
}
sideSpeed -= (30.0f * mPlayer->getRunVel().x);
}
if(mKeyboard->isKeyDown(OIS::KC_D))
{
if(mPlayer->getIsRunning() == false)
{
mPlayer->setRunVel(Ogre::Vector3(1,1,1));
}
sideSpeed += (30.0f * mPlayer->getRunVel().x);
}

if ((mKeyboard->isKeyDown(KC_LSHIFT)) && (mPlayer->getIsJumping() == false) && (mPlayer->getIsActive()))// jump
{
playerBody->addImpulse(playerBody->getOrientation() * Ogre::Vector3(0, 1, 0), playerBody->getPosition());
}

if ((mKeyboard->isKeyDown(KC_SPACE))&& (mPlayer->getIsJumping() == false) && (mPlayer->getStamina() > 1) && (mPlayer->getIsActive()))//run
{
mPlayer->setStamina(mPlayer->getStamina() - evt.timeSinceLastFrame);
mPlayer->setIsRunning(true);
if(mPlayer->getStamina() > 1)
{
mPlayer->setRunVel(Ogre::Vector3(2.5,2.5,2.5));
}
else
{
mPlayer->setRunVel(Ogre::Vector3(1,1,1));
}
}

if (mKeyboard->isKeyDown(OIS::KC_C))//crouch
{
if(cGraphicalInterface::getSingleton().getCameraHeight() > 3.0f)
cGraphicalInterface::getSingleton().setCameraHeight(cGraphicalInterface::getSingleton().getCameraHeight() - (evt.timeSinceLastFrame * 10));
}

//keys released
if (mKeyboard->isKeyDown(OIS::KC_C) == false)
{
if(cGraphicalInterface::getSingleton().getCameraHeight() < cGraphicalInterface::getSingleton().getInitialCameraHeight())
cGraphicalInterface::getSingleton().setCameraHeight(cGraphicalInterface::getSingleton().getCameraHeight() + (evt.timeSinceLastFrame * 10));
}

if(mKeyboard->isKeyDown(KC_SPACE) == false)
mPlayer->setIsRunning(false);

if (!m_cameraKeyState && mKeyboard->isKeyDown(OIS::KC_F3)) {
m_cameraMode = !m_cameraMode;
}
m_cameraKeyState = mKeyboard->isKeyDown(OIS::KC_F3);

Real timestep = evt.timeSinceLastFrame;
Real rate = (60.0f * 3.1416f/ 180.0f) * timestep;

// calculate the camera Pitch Angle
int pitchDir = -mMouse->getMouseState().Y.rel;
cameraPitchAngle += pitchDir * rate;
if (cameraPitchAngle > 80.0f * 3.1416f/ 180.0f) cameraPitchAngle = 80.0f * 3.1416f/ 180.0f;
if (cameraPitchAngle < -80.0f * 3.1416f/ 180.0f) cameraPitchAngle = -80.0f * 3.1416f/ 180.0f;
Ogre::Quaternion pitch (Ogre::Radian(cameraPitchAngle), Ogre::Vector3 (1, 0, 0));


int yawDir = -mMouse->getMouseState().X.rel;
heading += Ogre::Radian(yawDir * rate);

mPlayerController->setVelocity(forwardSpeed, sideSpeed, heading);

Ogre::Vector3 posit;
Ogre::Quaternion orient;

playerBody->getVisualPositionOrientation (posit, orient);

orient = orient * pitch;
if (m_cameraMode) {
// place the camera on external view
posit += orient * Ogre::Vector3 (0, 0, m_extenalViewDist);
}

// move eye point to be about the players head
posit.y += mPlayerController->getPlayerHeight() * cGraphicalInterface::getSingleton().getCameraHeight();
/*posit.y = 5;
posit.z = 100; //para debug
posit.x = 0;*/

cGraphicalInterface::getSingleton().getCamera()->setPosition(posit);
cGraphicalInterface::getSingleton().getCamera()->setOrientation(orient);

OgreNewt::Debugger& debug(cPhysicsInterface::getSingleton().getNewtWorld()->getDebugger());
if (mKeyboard->isKeyDown(OIS::KC_F3))
{
debug.startRaycastRecording();
debug.clearRaycastsRecorded();
debug.showDebugInformation();
}
else
{
debug.clearRaycastsRecorded();
debug.stopRaycastRecording();
debug.hideDebugInformation();
}

if (mKeyboard->isKeyDown(OIS::KC_T))
cPhysicsInterface::getSingleton().getNewtWorld()->setThreadCount( cPhysicsInterface::getSingleton().getNewtWorld()->getThreadCount() % 2 + 1);
}



bool cPlayerFrameListener::keyClicked(const OIS::KeyEvent &e)
{
return true;
}

bool cPlayerFrameListener::keyPressed(const OIS::KeyEvent &e)
{
return true;
}

bool cPlayerFrameListener::keyReleased(const OIS::KeyEvent &e)
{
return true;
}


bool cPlayerFrameListener::frameStarted(const FrameEvent& evt)
{
if(cGraphicalInterface::getSingleton().getWindow()->isClosed())
return false;

input(evt);
return true;
}

bool cPlayerFrameListener::frameEnded(const FrameEvent& evt)
{
if( mKeyboard->isKeyDown( KC_ESCAPE) )
{
return false;
}
return true;
}

cPlayerFrameListener::~cPlayerFrameListener()
{
//Remove ourself as a Window listener
WindowEventUtilities::removeWindowEventListener(cGraphicalInterface::getSingleton().getWindow(), this);
windowClosed(cGraphicalInterface::getSingleton().getWindow());
}

The important pieces of code for jump are:

This piece of code detects if the player is on the ground, if he is then he isn't jumping ( mPlayer->setIsJumping(false) ) otherwise...

//jumping
isOnTheGround = mEntityCollision->groundCollision(playerBody,mPlayerController->getPlayerHeight());

if(isOnTheGround)
mPlayer->setIsJumping(false);
else
mPlayer->setIsJumping(true);

This piece of code adds an impulse to the body only if the player is alive and if he isn't jumping:

if ((mKeyboard->isKeyDown(KC_LSHIFT)) && (mPlayer->getIsJumping() == false) && (mPlayer->getIsActive()))// jump
{
playerBody->addImpulse(playerBody->getOrientation() * Ogre::Vector3(0,1, 0), playerBody->getPosition());
}

I hope this help someone, I'm waiting for your feedback to know if was useful.

what is the cEntityCollisionHandler?

Note: I changed a bit the groundCollision function, i thin now it's more accurate.

It's the class were i deal with some custom collisions but you can implement the groundCollision function in your framelistener or in another class you desire.
By the way the jump depends of your gravity, mass, OgreNewt body size and impulse values, you need to fix them to work in your application since you must have different values.

can your share that?

Ok i will share the values.

weight = 80; //80 kg
mass = weight * 2 * 100; //this is a formula i made it's not a real physics formula but works for me
gravity = Ogre::Vector3(0,-9.8f,0);


I changed the crouch code to work with different values:

if (mKeyboard->isKeyDown(OIS::KC_C))//crouch
{
if(mPlayerController->getPlayerHeight() + cGraphicalInterface::getSingleton().getCameraHeight() > (mPlayerController->getPlayerHeight() + cGraphicalInterface::getSingleton().getInitialCameraHeight()) / 2)
cGraphicalInterface::getSingleton().setCameraHeight(cGraphicalInterface::getSingleton().getCameraHeight() - (evt.timeSinceLastFrame * mPlayer->getWeight()));
}

//keys released
if (mKeyboard->isKeyDown(OIS::KC_C) == false)
{
if(mPlayerController->getPlayerHeight() + cGraphicalInterface::getSingleton().getCameraHeight() < mPlayerController->getPlayerHeight() + cGraphicalInterface::getSingleton().getInitialCameraHeight())
cGraphicalInterface::getSingleton().setCameraHeight(cGraphicalInterface::getSingleton().getCameraHeight() + (evt.timeSinceLastFrame * mPlayer->getWeight()));
}

And i changed the impulse value because with a bigger body the player had to stay a little with the finger in the jump key until the body jumps because the impulse value wasn't good:

playerBody->addImpulse(playerBody->getOrientation() * Ogre::Vector3(0, 60, 0), playerBody->getPosition());

This is the way i setup the OgreNewt body:

void cPlayerCollisionHandler::setCustomForceCallback(OgreNewt::Body *body, float t1, int t2)
{
Ogre::Vector3 force = cPhysicsInterface::getSingleton().getGravity() * mEntity->getMass();
cPhysicsInterface::getSingleton().addLocalForce(body,force, Ogre::Vector3::ZERO);
}

OgreNewt::PlayerController* cPlayerCollisionHandler::createPlayerController(Ogre::Vector3 size, Ogre::SceneNode *node)
{
Ogre::Vector3 inertia, offset;
Ogre::Vector3 newSize(15.0f,25.0f,15.0f);//I'm only using the newsize variable to show the values i'm using, i use the Ogre::Vector3 size parameter in my code

OgreNewt::ConvexCollisionPtr col = OgreNewt::ConvexCollisionPtr(new OgreNewt::CollisionPrimitives::Ellipsoid( cPhysicsInterface::getSingleton().getNewtWorld(), newSize, 0 ));
OgreNewt::Body* body = cPhysicsInterface::getSingleton().createOgreNewtBody(col);
cPhysicsInterface::getSingleton().calculateInertialMatrix(col,inertia,offset);

#ifdef OGRENEWT_NO_COLLISION_SHAREDPTR
delete col;
#endif
cPhysicsInterface::getSingleton().assignBodyToNode(body,node);
cPhysicsInterface::getSingleton().setMassMatrix(body,cPhysicsInterface::getSingleton().getGravity().y,inertia);
cPhysicsInterface::getSingleton().setCenterOfMass(body,offset);
cPhysicsInterface::getSingleton().setPositionOrientation(body,node->getPosition(), Ogre::Quaternion::IDENTITY);
cPhysicsInterface::getSingleton().setAutoSleep(body,0);
body->setCustomForceAndTorqueCallback <cPlayerCollisionHandler>(&cPlayerCollisionHandler::setCustomForceCallback,this);
OgreNewt::PlayerController *mPlayerController = new OgreNewt::PlayerController(body, 0.4f);
mBody = body;
return mPlayerController;

}


If someone knows a better way to do this, fell free to post the better code.

majc, thanks for the code, it's helped me tremendously!

However, I do have one major issue: the jump code does not work for player controller if the body is op top of an object. If the player controller body is in mid air, I can make it pop up into the air further (since I'm not requiring the user to be on the ground to jump), but as soon as the player controller touches the floor it's "stuck". If I move forward and fall off the edge of my floor then I can then make the player controller "jump" higher in the air again. But of course once it lands again it's stuck.

If I comment out the creation of the player controller, then I can make the body jump all I want, even after it hits the floor. So it's something specific to adding the body to a player controller that stops it from working.

Any ideas?

- Houdini

In my solution is working well but i had that problem before and i think i know what is happening.
My first advice is to write the result of the collision(camRay.getHitCount()) to the screen or to a text file, in my case i write to a text file and see what is happening.
I think that the problem might be that it detects collision right before hitting the ground, but for some reason when there is no space between the player body and the ground he can't detect(I don't know why) what i made in that time was:

OgreNewt::BasicRaycast camRay(cPhysicsInterface::getSingleton().getNewtWorld(),Ogre::Vector3(pos.x,pos.y + 2,pos.z),Ogre::Vector3(pos.x,pos.y - 3.0f,pos.z),false);

I cast the ray instead of the bottom of the body some units before in this case 2 units for example, that way it has some space between the start of the ray and the floor.
It worked for me in that time but it's not the cleanest solution and i don't know if it's the same problem you are facing.
Maybe if you paste your code we can see what is happening.

thanks for your help

No problem i hope it help.

majc,

FYI, I found my exact issue in the Newton forums:
http://newtondynamics.com/forum/viewtop ... ump#p42997

I can "fix" the issue like this person did, by changing the stair factor to 0. Gonna go through the Newton code to see if I can figure out what needs to be fixed in there. But at least I have a way to get passed the issue!

- Houdini

majc,

FYI, I found my exact issue in the Newton forums:
http://newtondynamics.com/forum/viewtop ... ump#p42997

I can "fix" the issue like this person did, by changing the stair factor to 0. Gonna go through the Newton code to see if I can figure out what needs to be fixed in there. But at least I have a way to get passed the issue!

- Houdini

First of all thanks a lot for your contribution Houdini because the stair factor was f******* the controller :p now the controller is working perfectly.
This solved 2 problems, this we were talking and another one i had with collisions, every time i collide with a body, the player jumped over it, i never thought the stair factor was the problem because the other body was higher that 0.4f, clearly the stair factor of player controller it's not working well.
I will try in the future (when i need it) to implement a new one and if it works i will post here.
This is what i changed with the help that Houdini gave me:

OgreNewt::PlayerController *mPlayerController = new OgreNewt::PlayerController(body, 0.000001f); //was 0.4f we can change to 0 because gives a memory error when i hit the ogrenewt debug key

after that i adjusted the impulse value to work better with this changes:

playerBody->addImpulse(playerBody->getOrientation() * Ogre::Vector3(0, 20, 0), playerBody->getPosition()); //was 60

Note: the impulse value works perfectly with the values I'm using in my code, it must be changed if you use another values.

If someone ask i can paste here all the code i have now for controller but i think we have here all the code needed.

what about the cPlayer?

cPlayer only has setters and getters for some player properties:

#include "cPlayer.h"

cPlayer::cPlayer()
{
ID = 1;
name = "'Miguel'";
gang = "'Dreads'";
missionStatus = "'None'";
cam_size = Ogre::Vector3(1,1,1);
mIsActive = 1;
damage = 0;
jmpVel = 3;
isJumping = false;
isRunning = false;
staminaLimit = 10;
stamina = staminaLimit;
hitpoints = 100;
disease = "''";
position = Ogre::Vector3(0,0,0);
}

int cPlayer::getHitpoints()
{
return hitpoints;
}

Vector3 cPlayer::getCamSize()
{
return cam_size;
}

float cPlayer::getJmpVel()
{
return jmpVel;
}

bool cPlayer::getIsJumping()
{
return isJumping;
}

bool cPlayer::getIsRunning()
{
return isRunning;
}

Real cPlayer::getStamina()
{
return stamina;
}

Real cPlayer::getStaminaLimit()
{
return staminaLimit;
}

Vector3 cPlayer::getRunVel()
{
return runVel;
}

std::string cPlayer::getDisease()
{
return disease;
}

std::string cPlayer::getMissionStatus()
{
return missionStatus;
}

std::string cPlayer::getGang()
{
return gang;
}

void cPlayer::setHitpoints(int hitpoints)
{
this->hitpoints = hitpoints;
if (this->hitpoints < 0)
this->hitpoints = 0;
}

void cPlayer::setRunVel(Vector3 run)
{
runVel = run;
}

void cPlayer::setStaminaLimit(Real limit)
{
staminaLimit = limit;
}

void cPlayer::setStamina(Real stam)
{
stamina = stam;
}

void cPlayer::setIsRunning(bool isRun)
{
isRunning = isRun;
}

void cPlayer::setIsJumping(bool isJmp)
{
isJumping = isJmp;
}

void cPlayer::setJmpVel(float jpVel)
{
jmpVel = jpVel;
}

void cPlayer::setDisease(const std::string &disease)
{
this->disease = "'"+disease+"'";
}

void cPlayer::setMissionStatus(const std::string &missionStatus)
{
//have to be changed to suport a queue
this->missionStatus = "'"+missionStatus+"'";
}

void cPlayer::setGang(const std::string &gang)
{
this->gang = "'"+gang+"'";
}



cPlayer::~cPlayer()
{
}

I'm just starting out using ogrenewt (and ogre for that matter) so i've just been messing with the demos until now.

I've made some changes in the 9th ogrenewt demo (the egg moving trough some level) and i have successfully made a robot who can play soccer with an ogrehead.
reading this I think to jump the most important line would be: playerBody->addImpulse(playerBody->getOrientation()* Ogre::Vector3(0, 1, 0), playerBody->getPosition());
leaving the collision detection out for now, i've added this to my jump key in the frame listener. but the problem is, i can only jump when not touching the ground (or making the impuls so high that the body shoots up into the end of the world).

is there anything that i'm missing?

seems i had the same problem with stairheight.
now i'll just have to find out how to set it to 0.0001 when jumping and back to 0.4 when done jumping.

edit: i cant it to work, does anyone have any code at the ready to do this?
i hope someone actualy still reads this stuff. cause i kinda need it to work soon

Sjinta,

I modified Newton/OgreNewt to allow jumping. To jump you simply call addImpulse() which I added to the OgreNewt_PlayerController class, and set the jump boolean to true. For example:


player_controller->addImpulse(0, 0, 10, true);


Below is the modified files that add this functionality:

CustomPlayerContoller.h

/* Copyright (c) <2009> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/

#if !defined(AFX_CUSTOM_PLAYER_CONTROLLER_INCLUDED)
#define AFX_CUSTOM_PLAYER_CONTROLLER_INCLUDED


#include "NewtonCustomJoint.h"

class JOINTLIBRARY_API CustomPlayerController: public NewtonCustomJoint
{
public:
enum PlayerState
{
m_onLand,
m_onFreeFall,
m_onIlligalRamp,
//m_onJumping,
};

CustomPlayerController (const dMatrix& localFrame, const NewtonBody* child, dFloat maxStairStepFactor, dFloat cushion);
virtual ~CustomPlayerController();

dFloat GetMaxSlope () const;
void SetMaxSlope (dFloat maxSlopeAngleIndRadian);
void SetVelocity (dFloat forwardSpeed, dFloat sideSpeed, dFloat heading);
void GetVelocity (dFloat& forwardSpeed, dFloat& sideSpeed, dFloat& heading) const;
dMatrix CalculateVisualMatrix () const;

dFloat GetPlayerHeight() const;
dFloat GetPlayerStairHeight() const;
void SetPlayerStairHeight(dFloat stair_height);
void SetPlayerState(PlayerState state);
PlayerState GetPlayerState();

void AddImpulse(dFloat ximpulse, dFloat yimpulse, dFloat zimpulse, bool jump);
/*
virtual bool CanPushBody (const NewtonBody* hitBody) const {return true;}
const NewtonCollision* GetDynamicsSensorShape () const;
*/
const NewtonCollision* GetSensorShape () const;
const NewtonCollision* GetStairStepShape () const;


protected:
virtual void SubmitConstraints (dFloat timestep, int threadIndex);

private:



void PlayerOnLand (dFloat timestep, int threadIndex);
void PlayerOnRamp (dFloat timestep, int threadIndex);
void PlayerOnFreeFall (dFloat timestep, int threadIndex);
void KinematicMotion (dFloat timestep, int threadIndex);
int FindFloor (const dMatrix& origin, const dVector& dest, const dVector upDir, NewtonCollision* m_bodySensorShape, dFloat& hitParam, dVector& normal, int threadIndex) const;

int PreProcessContacts (NewtonWorldConvexCastReturnInfo* const contacts, int count, const dVector& updir) const;
dVector CalculateVelocity (const dVector& velocSrc, dFloat timestep, const dVector& upDir, dFloat elevation, int threadIndex) const;


static void KinematicMotion (const NewtonJoint* userJoint, dFloat timestep, int threadIndex);
static unsigned ConvexStaticCastPrefilter(const NewtonBody* body, const NewtonCollision* collision, void* userData);
static unsigned ConvexAllBodyCastPrefilter(const NewtonBody* body, const NewtonCollision* collision, void* userData);

protected:
struct CastFilterData
{
CastFilterData (const NewtonBody* me)
{
m_count = 1;
m_filter[0] = me;
}
int m_count;
const NewtonBody* m_filter[8];
};

/*
struct FindFloorData
{
FindFloorData (const NewtonBody* me)
:m_normal (0.0f, 1.0f, 0.0f, 0.0f)
{
m_me = me;
m_param = 2.0f;
m_hitBody = NULL;
}

dVector m_normal;
dFloat m_param;
const NewtonBody* m_me;
const NewtonBody* m_hitBody;
};
*/

dFloat m_heading;
dFloat m_loweCap;
dFloat m_maxSlope;
dFloat m_maxRadius;
dFloat m_sideSpeed;
dFloat m_restitution;
dFloat m_forwardSpeed;
dFloat m_playerHeight;
dFloat m_stairHeight;
dFloat m_kinematicCushion;
dVector m_gravity;
dMatrix m_localMatrix0;
dMatrix m_localMatrix1;
PlayerState m_playerState;

NewtonCollision* m_bodySensorShape;
NewtonCollision* m_stairSensorShape;
NewtonCollision* m_bodyFloorSensorShape;

bool m_jumping;
};

#endif


CustomPlayerController.cpp

/* Copyright (c) <2009> <Newton Game Dynamics>
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
*
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely
*/

#include "CustomJointLibraryStdAfx.h"
#include "CustomPlayerController.h"


#define MAX_CONTACTS 16
#define SENSOR_SHAPE_SEGMENTS 32
#define MAX_COLLISIONS_ITERATION 8


#define DEAD_RESIDUAL_HORIZONTAL_VELOC 0.1f

CustomPlayerController::CustomPlayerController(
const dMatrix& playerFrameInGlobalSpace,
const NewtonBody* child,
dFloat maxStairStepFactor,
dFloat cushion)
:NewtonCustomJoint(6, child, NULL)
{
dVector com;
dMatrix matrix;
NewtonCollision* shape;

// the minimum cushion is 1.0/64.0f of a unit
if (cushion < 1.0f/64.0f) {
cushion = 1.0f/64.0f;
}

m_heading = 0.0f;
m_sideSpeed = 0.0f;
m_restitution = 0.0f;
m_forwardSpeed = 0.0f;
// m_playerState = m_onLand;
m_playerState = m_onFreeFall;
m_kinematicCushion = cushion;

SetMaxSlope (30.0f * 3.141592f / 180.0f);

NewtonBodyGetMatrix (child, &matrix[0][0]);
NewtonBodyGetCentreOfMass (child, &com[0]);
com.m_w = 1.0f;

dMatrix pinMatrix (playerFrameInGlobalSpace);
pinMatrix.m_posit = matrix.TransformVector(com);
pinMatrix.m_posit.m_w = 1.0f;
CalculateLocalMatrix (pinMatrix, m_localMatrix0, m_localMatrix1);


// register the callback for tire integration
NewtonUserJointSetFeedbackCollectorCallback (m_joint, KinematicMotion);

// calculate the dimensions of the Player internal auxiliary shapes
shape = NewtonBodyGetCollision (child);

// calculate top and bottom side
dVector top;
dVector bottom;
dVector upDir (m_localMatrix0.m_front);
NewtonCollisionSupportVertex (shape, &upDir[0], &top[0]);
// top += upDir.Scale (m_kinematicCushion);

dVector downDir (m_localMatrix0[0].Scale (-1.0f));
NewtonCollisionSupportVertex (shape, &downDir[0], &bottom[0]);
// bottom += downDir.Scale (m_kinematicCushion);

m_playerHeight = (top - bottom) % upDir;

// set stairs step high as a percent of the player high
m_stairHeight = m_playerHeight * maxStairStepFactor;

// calculate the radius
dFloat r1;
dFloat r2;
dFloat floorRadios;
dVector radiusVector1;
dVector radiusVector2;
NewtonCollisionSupportVertex (shape, &m_localMatrix0[1][0], &radiusVector1[0]);
NewtonCollisionSupportVertex (shape, &m_localMatrix0[2][0], &radiusVector2[0]);
r1 = dAbs (radiusVector1 % m_localMatrix0[1]);
r2 = dAbs (radiusVector2 % m_localMatrix0[2]);
floorRadios = (r1 > r2 ? r1 : r2);
m_maxRadius = floorRadios + m_kinematicCushion;

dVector stairSensorShape[SENSOR_SHAPE_SEGMENTS * 2];
dVector bodySensorPoints[SENSOR_SHAPE_SEGMENTS * 2];
dVector floorSensorShape[SENSOR_SHAPE_SEGMENTS * 2];

dFloat h0;
dFloat h1;
dFloat startHight;
h0 = bottom % pinMatrix[0];
h1 = top % pinMatrix[0] - m_stairHeight;
startHight = h0 + m_stairHeight;

m_loweCap = h0;
for (int i = 0; i < SENSOR_SHAPE_SEGMENTS; i ++) {

dFloat x;
dFloat z;
dFloat fx;
dFloat fz;

x = dCos (2.0f * 3.14159265f * dFloat(i) / dFloat(SENSOR_SHAPE_SEGMENTS));
z = dSin (2.0f * 3.14159265f * dFloat(i) / dFloat(SENSOR_SHAPE_SEGMENTS));

fx = floorRadios * x;
fz = floorRadios * z;

x = m_maxRadius * x;
z = m_maxRadius * z;

dVector point (h0, x, z);
point = m_localMatrix0.RotateVector (point);
bodySensorPoints[i].m_x = point.m_x;
bodySensorPoints[i].m_y = point.m_y;
bodySensorPoints[i].m_z = point.m_z;

point = dVector (h1, x, z);
point = m_localMatrix0.RotateVector (point);
bodySensorPoints[i + SENSOR_SHAPE_SEGMENTS].m_x = point.m_x;
bodySensorPoints[i + SENSOR_SHAPE_SEGMENTS].m_y = point.m_y;
bodySensorPoints[i + SENSOR_SHAPE_SEGMENTS].m_z = point.m_z;

point = dVector (h0, fx, fz);
point = m_localMatrix0.RotateVector (point);
floorSensorShape[i].m_x = point.m_x;
floorSensorShape[i].m_y = point.m_y;
floorSensorShape[i].m_z = point.m_z;

point = dVector (h1, fx, fz);
point = m_localMatrix0.RotateVector (point);
floorSensorShape[i + SENSOR_SHAPE_SEGMENTS].m_x = point.m_x;
floorSensorShape[i + SENSOR_SHAPE_SEGMENTS].m_y = point.m_y;
floorSensorShape[i + SENSOR_SHAPE_SEGMENTS].m_z = point.m_z;


point = dVector (h0, x, z);
point = m_localMatrix0.RotateVector (point);
stairSensorShape[i].m_x = point.m_x;
stairSensorShape[i].m_y = point.m_y;
stairSensorShape[i].m_z = point.m_z;

point = dVector (startHight, x, z);
point = m_localMatrix0.RotateVector (point);
stairSensorShape[i + SENSOR_SHAPE_SEGMENTS].m_x = point.m_x;
stairSensorShape[i + SENSOR_SHAPE_SEGMENTS].m_y = point.m_y;
stairSensorShape[i + SENSOR_SHAPE_SEGMENTS].m_z = point.m_z;

}

m_stairSensorShape = NewtonCreateConvexHull (m_world, SENSOR_SHAPE_SEGMENTS * 2, &stairSensorShape[0].m_x, sizeof (dVector), 0.0f, 0, NULL);
m_bodySensorShape = NewtonCreateConvexHull (m_world, SENSOR_SHAPE_SEGMENTS * 2, &bodySensorPoints[0].m_x, sizeof (dVector), 0.0f, 0, NULL);
m_bodyFloorSensorShape = NewtonCreateConvexHull (m_world, SENSOR_SHAPE_SEGMENTS * 2, &floorSensorShape[0].m_x, sizeof (dVector), 0.0f, 0, NULL);

m_jumping = false;
}

CustomPlayerController::~CustomPlayerController()
{
NewtonReleaseCollision(m_world, m_bodySensorShape);
NewtonReleaseCollision(m_world, m_stairSensorShape);
NewtonReleaseCollision(m_world, m_bodyFloorSensorShape);

}


void CustomPlayerController::SetVelocity (dFloat forwardSpeed, dFloat sideSpeed, dFloat heading)
{
m_heading = heading;
m_sideSpeed = sideSpeed;
m_forwardSpeed = forwardSpeed;
}

void CustomPlayerController::GetVelocity (dFloat& forwardSpeed, dFloat& sideSpeed, dFloat& heading) const
{
heading = m_heading;
sideSpeed = m_sideSpeed;
forwardSpeed = m_forwardSpeed;
}


const NewtonCollision* CustomPlayerController::GetSensorShape () const
{
return m_bodySensorShape;
}

const NewtonCollision* CustomPlayerController::GetStairStepShape () const
{
return m_stairSensorShape;
}



void CustomPlayerController::SetMaxSlope (dFloat maxSlopeAngleIndRadian)
{
//
maxSlopeAngleIndRadian = dAbs(maxSlopeAngleIndRadian);
if (maxSlopeAngleIndRadian < 10.0f * 3.14159265f / 180.0f) {
maxSlopeAngleIndRadian = 10.0f * 3.14159265f / 180.0f;
}
if (maxSlopeAngleIndRadian > 60.0f * 3.14159265f / 180.0f) {
maxSlopeAngleIndRadian = 60.0f * 3.14159265f / 180.0f;
}

m_maxSlope = dCos (maxSlopeAngleIndRadian);
// m_maxSlope = 1.0f - dSin (dAbs(maxSlopeAngleIndRadian));
}

dFloat CustomPlayerController::GetMaxSlope () const
{
// return dSin (1.0f - m_maxSlope);
return dCos (m_maxSlope);
}

dFloat CustomPlayerController::GetPlayerHeight() const
{
return m_playerHeight;
}

dFloat CustomPlayerController::GetPlayerStairHeight() const
{
return m_stairHeight;
}

void CustomPlayerController::SetPlayerStairHeight(dFloat stair_height)
{
m_stairHeight = stair_height;
}

void CustomPlayerController::SetPlayerState(PlayerState state)
{
m_playerState = state;
}

CustomPlayerController::PlayerState CustomPlayerController::GetPlayerState()
{
return m_playerState;
}

/*
void CustomPlayerController::GetInfo (NewtonJointRecord* info) const
{
}

unsigned CustomPlayerController::ConvexDynamicCastPrefilter(const NewtonBody* body, const NewtonCollision* collision, void* userData)
{
// for now just collide with static bodies
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
CastFilterData* CastFilterData;

CastFilterData = (CastFilterData*) userData;
for (int i =0; i < CastFilterData->m_count;i ++){
if (CastFilterData->m_filter[i] == body) {
return 0;
}
}
NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);
return (mass > 0.0f);
}


dFloat CustomPlayerController::FindFloorCallback(const NewtonBody* body, const dFloat* hitNormal, int collisionID, void* userData, dFloat intersetParam)
{
dFloat param;

param = 1.0f;
FindFloorData& data = *((FindFloorData*)userData);
if (body != data.m_me) {
param = intersetParam;
if (param < data.m_param) {
data.m_param = param;
data.m_normal.m_x = hitNormal[0];
data.m_normal.m_y = hitNormal[1];
data.m_normal.m_z = hitNormal[2];
data.m_hitBody = body;
}
}

return param;
}
*/

unsigned CustomPlayerController::ConvexStaticCastPrefilter(const NewtonBody* body, const NewtonCollision* collision, void* userData)
{
// for now just collide with static bodies
// dFloat mass;
// dFloat Ixx;
// dFloat Iyy;
// dFloat Izz;
// CastFilterData* CastFilterData;
// CastFilterData = (CastFilterData*) userData;
// for (int i =0; i < CastFilterData->m_count;i ++){
// if (CastFilterData->m_filter[i] == body) {
// return 0;
// }
// }
// NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);
// return (mass == 0.0f);
// return 1;

dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);
return (mass == 0.0f);
}

unsigned CustomPlayerController::ConvexAllBodyCastPrefilter(const NewtonBody* body, const NewtonCollision* collision, void* userData)
{
CastFilterData* filterData;

filterData = (CastFilterData*) userData;
for (int i =0; i < filterData->m_count;i ++){
if (filterData->m_filter[i] == body) {
return 0;
}
}
return 1;
}




void CustomPlayerController::KinematicMotion (const NewtonJoint* userJoint, dFloat timestep, int threadIndex)
{
CustomPlayerController* joint;

// get the pointer to the joint class
joint = (CustomPlayerController*) NewtonJointGetUserData (userJoint);
joint->KinematicMotion(timestep, threadIndex);
}


dMatrix CustomPlayerController::CalculateVisualMatrix () const
{
dMatrix bodyMatrix;

// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &bodyMatrix[0][0]);

/*
if (!m_isInJumpState) {
// dFloat hitParam;
// CastFilterData staticCastFilterData (m_body0);
// NewtonWorldConvexCastReturnInfo info;
dVector upDir (bodyMatrix.RotateVector (m_localMatrix0.m_front));
// bodyMatrix.m_posit += upDir.Scale (m_stairHeight);
// dVector target (bodyMatrix.m_posit - upDir.Scale(m_stairHeight * 2.0f));

FindFloorData floor (m_body0);
dVector p0 (bodyMatrix.m_posit + upDir.Scale (m_loweCap + m_stairHeight));
dVector p1 (p0 - upDir.Scale (2.0f * m_stairHeight));
NewtonWorldRayCast (m_world, &p0[0], &p1[0], FindFloorCallback, &floor, NULL);
if (floor.m_hitBody) {
bodyMatrix.m_posit -= upDir.Scale (m_stairHeight * (2.0f * floor.m_param - 1.0f));
}
}
*/
return bodyMatrix;
}





void CustomPlayerController::SubmitConstraints (dFloat timestep, int threadIndex)
{
dFloat mag;
dFloat angle;
dFloat invIxx;
dFloat invIyy;
dFloat invIzz;
dFloat invMass;
dMatrix matrix0;
dMatrix matrix1;
dVector velocity;

// save the gravity before collision force are applied
NewtonBodyGetForceAcc (m_body0, &m_gravity[0]);
NewtonBodyGetInvMass(m_body0, &invMass, &invIxx, &invIyy, &invIzz);
m_gravity = m_gravity.Scale (invMass);


// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, matrix0, matrix1);

// if the body ha rotated by some amount, the there will be a plane of rotation
dVector lateralDir (matrix0.m_front * matrix1.m_front);
mag = lateralDir % lateralDir;
if (mag > 1.0e-6f) {
// if the side vector is not zero, it means the body has rotated
mag = dSqrt (mag);
lateralDir = lateralDir.Scale (1.0f / mag);
angle = dAsin (mag);

// add an angular constraint to correct the error angle
NewtonUserJointAddAngularRow (m_joint, angle, &lateralDir[0]);

// in theory only one correction is needed, but this produces instability as the body may move sideway.
// a lateral correction prevent this from happening.
dVector frontDir (lateralDir * matrix1.m_front);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &frontDir[0]);
} else {
// if the angle error is very small then two angular correction along the plane axis do the trick
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
}



// get linear velocity
if (m_playerState == m_onLand) {
// dFloat mag2;
NewtonBodyGetVelocity(m_body0, &velocity[0]);

// Get the global matrices of each rigid body.
const dVector& frontDir = matrix0.m_up;
const dVector& strafeDir = matrix0.m_right;
const dVector& upDir = matrix0.m_front;
dVector desiredVeloc (frontDir.Scale (m_forwardSpeed) + upDir.Scale (velocity % upDir) + strafeDir.Scale (-m_sideSpeed));
// dFloat maxSpeed = dAbs (m_forwardSpeed) > dAbs (m_sideSpeed) ? dAbs (m_forwardSpeed) : dAbs (m_sideSpeed);
// mag2 = desiredVeloc % desiredVeloc;
// if (mag2 > 1.0e-6f) {
// desiredVeloc = desiredVeloc.Scale (maxSpeed /dSqrt (mag2));
// }

NewtonBodySetVelocity(m_body0, &desiredVeloc[0]);
}

}


int CustomPlayerController::PreProcessContacts (NewtonWorldConvexCastReturnInfo* const contacts, int count, const dVector& upDir) const
{
// flatten contact prune contacts
for (int i = 0; i < count; i ++) {
dVector normal (contacts[i].m_normal);
// dVector point (contacts[i].m_point);
normal -= upDir.Scale (upDir % normal);
normal = normal.Scale (1.0f / dSqrt (normal % normal));
contacts[i].m_normal[0] = normal.m_x;
contacts[i].m_normal[1] = normal.m_y;
contacts[i].m_normal[2] = normal.m_z;
// contacts[i].m_normal[3] = - (normal % point);
}

// prune contact with the same normal
for (int i = 0; i < (count - 1); i ++) {
const dVector& normal0 = *((dVector*) (&contacts[i].m_normal[0]));
for (int j = i + 1; j < count; j ++) {
const dVector& normal1 = *((dVector*) (&contacts[j].m_normal[0]));
dFloat val = normal0 % normal1;
if (val > 0.9995f) {
count --;
contacts[j] = contacts[count];
j --;
}
}
}
return count;
}

dVector CustomPlayerController::CalculateVelocity (
const dVector& veloc,
dFloat timestep,
const dVector& upDir,
dFloat elevation,
int threadIndex) const
{
// see if it hit and obstacle

dFloat mag2;

dVector verticalVeloc (upDir.Scale (veloc % upDir)) ;
dVector velocity (veloc - verticalVeloc);
mag2 = velocity % velocity;

/*
static int xxx1;
xxx1 ++;
if (xxx1 >= 170){
xxx1 *=1;
dMatrix bodyMatrix;

//NewtonBodyGetVelocity(m_body0, &veloc[0]);
NewtonBodyGetMatrix(m_body0, &bodyMatrix[0][0]);
static dVector xxxx(bodyMatrix.m_posit);
dVector x (bodyMatrix.m_posit - xxxx);
//dTrace (("%d %f %f %f\n", xxx, bodyMatrix.m_posit.m_x, bodyMatrix.m_posit.m_y, bodyMatrix.m_posit.m_z));
dTrace (("step(%d %f %f %f) v(%f %f)\n", xxx1, x.m_x, x.m_y, x.m_z, veloc.m_x, veloc.m_z));
xxxx = bodyMatrix.m_posit;
}
*/

if (mag2 > 0.0f) {
int contactCount;
int prevContactCount;
int positionHasChanged;
dFloat hitParam;
dFloat paddingTimestep;
dMatrix bodyMatrix;
CastFilterData castFilterData (m_body0);
NewtonWorldConvexCastReturnInfo info[MAX_CONTACTS];
NewtonWorldConvexCastReturnInfo prevInfo[MAX_CONTACTS];


NewtonBodyGetMatrix(m_body0, &bodyMatrix[0][0]);
castFilterData.m_count = 1;
bodyMatrix.m_posit += upDir.Scale (elevation);

dVector translationStep (velocity.Scale (timestep));
dVector paddingStep (velocity.Scale (m_kinematicCushion / dSqrt (mag2)));
dVector destination (bodyMatrix.m_posit + translationStep + paddingStep);

paddingTimestep = (paddingStep % velocity) / (velocity % velocity);

prevContactCount = 0;
positionHasChanged = 0;
contactCount = NewtonWorldConvexCast (m_world, &bodyMatrix[0][0], &destination[0], m_bodySensorShape, &hitParam, &castFilterData, ConvexStaticCastPrefilter, info, sizeof (info) / sizeof (info[0]), threadIndex);
contactCount = PreProcessContacts (info, contactCount, upDir);
for (int iterations = 0; contactCount && (iterations < MAX_COLLISIONS_ITERATION); iterations ++) {
int flag;
int count;
dFloat time;

// calculate the travel time, and subtract from time remaining
time = hitParam * ((translationStep + paddingStep) % velocity) / (velocity % velocity) - paddingTimestep;

// teleport the body to the intersection point
positionHasChanged = 1;
bodyMatrix.m_posit += velocity.Scale (time);

dFloat speed[MAX_CONTACTS * 2];
dFloat bounceSpeed[MAX_CONTACTS * 2];
dVector bounceNormal[MAX_CONTACTS * 2];
dVector auxBounceVeloc (0.0f, 0.0f, 0.0f, 0.0f);

count = 0;
for (int i = 0; i < contactCount; i ++) {
dFloat reboundVeloc;

speed[count] = 0.0f;

bounceNormal[count] = info[i].m_normal;
reboundVeloc = -(velocity % bounceNormal[count]) * (1.0f + m_restitution);
bounceSpeed[count] = (reboundVeloc > 0.0f) ? reboundVeloc : 0.0f;
count ++;
}

for (int i = 0; i < prevContactCount; i ++) {
dFloat reboundVeloc;

speed[count] = 0.0f;
bounceNormal[count] = prevInfo[i].m_normal;
reboundVeloc = (velocity % bounceNormal[count]) * (1.0f + m_restitution);
bounceSpeed[count] = (reboundVeloc > 0.0f) ? reboundVeloc : 0.0f;
count ++;
}

dFloat residual;
residual = 10.0f;
for (int i = 0; (i < 16) && (residual > 1.0e-3f); i ++) {
residual = 0.0f;
for (int k = 0; k < count; k ++) {
dFloat v;
dFloat x;

v = bounceSpeed[k] - bounceNormal[k] % auxBounceVeloc;
x = speed[k] + v;

if (x < 0.0f) {
v = 0.0f;
x = 0.0f;
}

if (dAbs (v) > residual) {
residual = dAbs (v);
}

auxBounceVeloc += bounceNormal[k].Scale (x - speed[k]);
speed[k] = x;
}
}

flag = 0;
for (int i = 0; i < count; i ++) {
flag = (speed[i] > 0.0f);
velocity += bounceNormal[i].Scale (speed[i]);
}

prevContactCount = contactCount;
contactCount = 0;
if (flag) {
memcpy (prevInfo, info, prevContactCount * sizeof (NewtonWorldConvexCastReturnInfo));
mag2 = velocity % velocity;
if (mag2 > 1.0e-6f) {
translationStep = velocity.Scale (timestep);
paddingStep = velocity.Scale (m_kinematicCushion / dSqrt (mag2));
dVector destination (bodyMatrix.m_posit + translationStep + paddingStep);
castFilterData.m_count = 1;

contactCount = NewtonWorldConvexCast (m_world, &bodyMatrix[0][0], &destination[0], m_bodySensorShape, &hitParam, &castFilterData, ConvexStaticCastPrefilter, info, sizeof (info) / sizeof (info[0]), threadIndex);
contactCount = PreProcessContacts (info, contactCount, upDir);
}
}
}

if (positionHasChanged) {
bodyMatrix.m_posit -= upDir.Scale (elevation);
NewtonBodySetMatrix (m_body0, &bodyMatrix[0][0]);
}
// prevent the body form drifting form residual velocity when it is in a corner
mag2 = velocity % velocity;
if (mag2 < (DEAD_RESIDUAL_HORIZONTAL_VELOC * DEAD_RESIDUAL_HORIZONTAL_VELOC)) {
velocity = dVector (0.0f, 0.0f, 0.0f, 0.0f) ;
}
}

return (velocity + verticalVeloc);
}

int CustomPlayerController::FindFloor (const dMatrix& origin, const dVector& dest, const dVector upDir, NewtonCollision* shape, dFloat& hitParam, dVector& normal, int threadIndex) const
{
int contacts;
CastFilterData castFilterData (m_body0);
NewtonWorldConvexCastReturnInfo info[MAX_CONTACTS];

contacts = NewtonWorldConvexCast (m_world, &origin[0][0], &dest[0], shape, &hitParam, &castFilterData, ConvexAllBodyCastPrefilter, info, 8, threadIndex);
if (contacts) {
int bestContact = 0;
dFloat bestValue = upDir % info[0].m_normalOnHitPoint;
for (int i = 1; i < contacts; i ++) {
dFloat value = upDir % info[i].m_normalOnHitPoint;
if (value > bestValue) {
bestContact = i;
bestValue = value;

}
}
normal = info[bestContact].m_normalOnHitPoint;
contacts = 1;
}
return contacts;
}


void CustomPlayerController::PlayerOnFreeFall (dFloat timestep, int threadIndex)
{
dFloat dist;
dFloat hitParam;
dVector velocity;
dMatrix bodyMatrix;

// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &bodyMatrix[0][0]);
dVector posit (bodyMatrix.m_posit);

dVector upDir (bodyMatrix.RotateVector (m_localMatrix0.m_front));
dVector frontDir (bodyMatrix.RotateVector (m_localMatrix0.m_up));

// make sure the body velocity will no penetrates other bodies
NewtonBodyGetVelocity (m_body0, &velocity[0]);

velocity = CalculateVelocity (velocity, timestep, upDir, m_stairHeight, threadIndex);

// player of in free fall look ahead for the land
dist = upDir % velocity.Scale (timestep);

bodyMatrix.m_posit -= upDir.Scale (m_kinematicCushion);
dVector floorNormal;
dVector target (bodyMatrix.m_posit + upDir.Scale (dist));
if (FindFloor (bodyMatrix, target, upDir, m_bodyFloorSensorShape, hitParam, floorNormal, threadIndex)) {
// player is about to land, snap position to the ground
bodyMatrix.m_posit = posit + upDir.Scale (dist * hitParam);
NewtonBodySetMatrix (m_body0, &bodyMatrix[0][0]);

if ((floorNormal % upDir) < m_maxSlope) {
m_playerState = m_onIlligalRamp;
} else {
m_playerState = m_onLand;
velocity = velocity - upDir.Scale (upDir % velocity);
}
}
NewtonBodySetVelocity(m_body0, &velocity[0]);
m_jumping = false;
}



void CustomPlayerController::PlayerOnRamp (dFloat timestep, int threadIndex)
{
dFloat hitParam;
dVector velocity;
dMatrix bodyMatrix;
// CastFilterData castFilterData (m_body0);
// NewtonWorldConvexCastReturnInfo info[MAX_CONTACTS];

// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &bodyMatrix[0][0]);
dVector posit (bodyMatrix.m_posit);

dVector upDir (bodyMatrix.RotateVector (m_localMatrix0.m_front));
dVector frontDir (bodyMatrix.RotateVector (m_localMatrix0.m_up));

// make sure the body velocity will no penetrates other bodies
NewtonBodyGetVelocity (m_body0, &velocity[0]);


// apply free fall gravity force to the body along the ramp
// castFilterData.m_count = 1;
bodyMatrix.m_posit = posit + upDir.Scale (m_stairHeight);
dVector target (bodyMatrix.m_posit - upDir.Scale(m_stairHeight * 2.0f));
dVector floorNormal;
// if (NewtonWorldConvexCast (m_world, &bodyMatrix[0][0], &target[0], m_bodyFloorSensorShape, &hitParam, &castFilterData, ConvexStaticCastPrefilter, info, sizeof (info) / sizeof (info[0]), threadIndex)) {
if (FindFloor (bodyMatrix, target, upDir, m_bodyFloorSensorShape, hitParam, floorNormal, threadIndex)) {
// velocity -= m_gravity.Scale (timestep);
velocity += m_gravity.Scale (timestep);

// dVector floorNormal (info[0].m_normalOnHitPoint[0], info[0].m_normalOnHitPoint[1], info[0].m_normalOnHitPoint[2], 0.0f);
// dVector gravityForce (m_gravity - floorNormal.Scale (floorNormal % m_gravity));
// velocity = velocity - floorNormal.Scale (floorNormal % velocity) + gravityForce.Scale (timestep);
velocity = velocity - floorNormal.Scale (floorNormal % velocity);
}

velocity = CalculateVelocity (velocity, timestep, upDir, m_stairHeight, threadIndex);

// if the player did not change state, then make sure it is still landed on a floor
dVector step (velocity.Scale (timestep));
bodyMatrix.m_posit = posit + step + upDir.Scale(m_stairHeight - m_kinematicCushion);
target = bodyMatrix.m_posit - upDir.Scale(m_stairHeight * 2.0f);

// castFilterData.m_count = 1;
// if (NewtonWorldConvexCast (m_world, &bodyMatrix[0][0], &target[0], m_bodyFloorSensorShape, &hitParam, &castFilterData, ConvexAllBodyCastPrefilter, info, 1, threadIndex)) {
if (FindFloor (bodyMatrix, target, upDir, m_bodyFloorSensorShape, hitParam, floorNormal, threadIndex)) {
if (hitParam > 1.0e-3f) {

int isFuturePosiInRamp;

bodyMatrix.m_posit = bodyMatrix.m_posit + (target - bodyMatrix.m_posit).Scale (hitParam) - step + upDir.Scale (m_kinematicCushion);
NewtonBodySetMatrix (m_body0, &bodyMatrix[0][0]);

// dVector floorNormal (info[0].m_normalOnHitPoint[0], info[0].m_normalOnHitPoint[1], info[0].m_normalOnHitPoint[2], 0.0f);

isFuturePosiInRamp = (floorNormal % upDir) < m_maxSlope;
if (!isFuturePosiInRamp) {
m_playerState = m_onLand;
// } else {
// // apply free fall gravity force to the body along the ramp
// dFloat hitParam1;
// NewtonWorldConvexCastReturnInfo info1;
// bodyMatrix.m_posit = posit + upDir.Scale (m_stairHeight - m_kinematicCushion);
// dVector target (bodyMatrix.m_posit - upDir.Scale(m_stairHeight * 2.0f));
// NewtonWorldConvexCast (m_world, &bodyMatrix[0][0], &target[0], m_bodyFloorSensorShape, &hitParam1, &castFilterData, ConvexAllBodyCastPrefilter, &info1, 1, threadIndex);
// if (hitParam < hitParam1) {
// // the player hit the edge of a forbidden ramp
// //floorNormal -= upDir.Scale (floorNormal % upDir);
// //_ASSERTE ((floorNormal % floorNormal) > 1.0e-3f);
// //floorNormal = floorNormal.Scale (1.0f / dSqrt (floorNormal % floorNormal));
// velocity -= floorNormal.Scale (velocity % floorNormal);
// }
}
}
} else {
m_playerState = m_onFreeFall;
}

NewtonBodySetVelocity(m_body0, &velocity[0]);
}



void CustomPlayerController::PlayerOnLand (dFloat timestep, int threadIndex)
{
dFloat hitParam;
dVector velocity;
dMatrix bodyMatrix;

// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &bodyMatrix[0][0]);
dVector posit (bodyMatrix.m_posit);

dVector upDir (bodyMatrix.RotateVector (m_localMatrix0.m_front));
dVector frontDir (bodyMatrix.RotateVector (m_localMatrix0.m_up));

// enlarge the time step to no overshot too much

// make sure the body velocity will no penetrates other bodies
NewtonBodyGetVelocity (m_body0, &velocity[0]);

// subtract the Gravity contribution
velocity -= m_gravity.Scale (timestep);
velocity = CalculateVelocity (velocity, timestep, upDir, m_stairHeight, threadIndex);

// if the player did not change state, then make sure it is still landed on a floor
dVector step (velocity.Scale (timestep));
dVector target;
if (!m_jumping)
{
bodyMatrix.m_posit = posit + step + upDir.Scale (m_stairHeight - m_kinematicCushion);
target = (bodyMatrix.m_posit - upDir.Scale(m_stairHeight * 2.0f));
}
else
{
bodyMatrix.m_posit = posit + step + upDir.Scale (0.0f - m_kinematicCushion);
target = (bodyMatrix.m_posit - upDir.Scale(0.0f * 2.0f));
}

dVector floorNormal;
if (FindFloor (bodyMatrix, target, upDir, m_bodyFloorSensorShape, hitParam, floorNormal, threadIndex)) {
if (hitParam > 1.0e-3f) {
int isFuturePosiInRamp;
isFuturePosiInRamp = (floorNormal % upDir) < m_maxSlope;

if (isFuturePosiInRamp) {
// apply free fall gravity force to the body along the ramp
dFloat hitParam1;
dVector floorNormal1;

bodyMatrix.m_posit = posit + upDir.Scale (m_stairHeight - m_kinematicCushion);
target = bodyMatrix.m_posit - upDir.Scale(m_stairHeight * 2.0f);
FindFloor (bodyMatrix, target, upDir, m_bodyFloorSensorShape, hitParam1, floorNormal1, threadIndex);
if (hitParam < hitParam1) {

int iterations;
int contactCount = 1;
// the player hit the edge of a forbidden ramp
dVector savedVeloc (velocity);
for (iterations = 0; contactCount && (iterations < MAX_COLLISIONS_ITERATION); iterations ++) {
dFloat projectVel;
floorNormal -= upDir.Scale (floorNormal % upDir);
_ASSERTE ((floorNormal % floorNormal) > 1.0e-3f);
floorNormal = floorNormal.Scale (1.0f / dSqrt (floorNormal % floorNormal));

projectVel = velocity % floorNormal;
velocity -= floorNormal.Scale (projectVel);

step = velocity.Scale (timestep);
bodyMatrix.m_posit = posit + step + upDir.Scale (m_stairHeight - m_kinematicCushion);
target = bodyMatrix.m_posit - upDir.Scale(m_stairHeight * 2.0f);

if (FindFloor (bodyMatrix, target, upDir, m_bodyFloorSensorShape, hitParam, floorNormal, threadIndex)) {
contactCount = (floorNormal % upDir) < m_maxSlope;
}
}

if (iterations >= MAX_COLLISIONS_ITERATION) {
dVector veloc1 (CalculateVelocity (savedVeloc, timestep, upDir, m_stairHeight, threadIndex));
dVector err (veloc1 - velocity);
if ((err % err) < 1.0e-6f) {
m_playerState = m_onIlligalRamp;
} else {
velocity = veloc1;
hitParam = 0.0f;
step = dVector (0.0f, 0.0f, 0.0f, 0.0f);
bodyMatrix.m_posit = posit - upDir.Scale (m_kinematicCushion);
}
}

} else {
m_playerState = m_onIlligalRamp;
}
}

bodyMatrix.m_posit = bodyMatrix.m_posit + (target - bodyMatrix.m_posit).Scale (hitParam) + upDir.Scale (m_kinematicCushion);
bodyMatrix.m_posit -= step;
NewtonBodySetMatrix (m_body0, &bodyMatrix[0][0]);
}
} else {
m_playerState = m_onFreeFall;
}

NewtonBodySetVelocity(m_body0, &velocity[0]);
}



void CustomPlayerController::KinematicMotion (dFloat timestep, int threadIndex)
{
dFloat turnOmega;
dFloat turnAngle;
dMatrix bodyMatrix;

// handle angular velocity and heading
NewtonBodyGetMatrix(m_body0, &bodyMatrix[0][0]);

dVector upDir (bodyMatrix.RotateVector (m_localMatrix0.m_front));
dVector frontDir (bodyMatrix.RotateVector (m_localMatrix0.m_up));

dVector heading (0.0f, dCos (m_heading), dSin (m_heading), 0.0f);
heading = m_localMatrix0.RotateVector(heading);
turnAngle = (frontDir * heading) % upDir;
turnAngle = dAsin (min (max (turnAngle, -1.0f), 1.0f));
turnOmega = turnAngle / timestep;

dVector omega (upDir.Scale (turnOmega));
NewtonBodySetOmega(m_body0, &omega.m_x);

switch (m_playerState)
{
case m_onFreeFall:
PlayerOnFreeFall (timestep, threadIndex);
break;

case m_onLand:
PlayerOnLand (timestep, threadIndex);
break;

case m_onIlligalRamp:
PlayerOnRamp (timestep, threadIndex);
break;
}
}



void CustomPlayerController::AddImpulse(dFloat ximpulse, dFloat yimpulse, dFloat zimpulse, bool jump)
{
dMatrix bodyMatrix;

// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &bodyMatrix[0][0]);

dVector velocity(ximpulse, yimpulse, zimpulse);
dVector posit(bodyMatrix.m_posit);

NewtonBodyAddImpulse(m_body0, &velocity[0], &posit[0]);
m_jumping = jump;
}


OgreNewt_PlayerController.h

/*
OgreNewt Library

Ogre implementation of Newton Game Dynamics SDK

OgreNewt basically has no license, you may use any or all of the library however you desire... I hope it can help you in any way.

by melven

*/
#ifndef _INCLUDE_OGRENEWT_PLAYERCONTROLLER
#define _INCLUDE_OGRENEWT_PLAYERCONTROLLER


#include "OgreNewt_Prerequisites.h"
#include "OgreNewt_Joint.h"
#include "OgreNewt_RayCast.h"
#include "OgreNewt_Body.h"


namespace OgreNewt
{
//! PlayerController
/*!
this class implements a player-controller based on the code of the CustomPlayerController-class in the Newton-CustomJoints library
*/
class _OgreNewtExport PlayerController : public Joint
{
public:
//! constructor
/*!
* \param localFrame this is the player frame in global Space (x: side, y: up, -z forward)
*/
PlayerController(OgreNewt::Body* child, Ogre::Real stairHeight, Ogre::Real max_slope = 30.0f, Ogre::Real kinematicCushion = 1.0f/64.0f);
virtual ~PlayerController();

//! get currently set velocity
void getVelocity(Ogre::Real &forwardSpeed, Ogre::Real& sideSpeed, Ogre::Radian& heading) const;

//! set the characters velocity, the -Speed-values can be negative, sideSpeed positive means move to the right, heading is in absolute space
void setVelocity(Ogre::Real forwardSpeed, Ogre::Real sideSpeed, Ogre::Radian heading);

//! get the Player height
Ogre::Real getPlayerHeight() const;

void setStairHeight(Ogre::Real height);
void setPlayerState(Ogre::uint32 state);
Ogre::uint32 getPlayerState();
void addImpulse(Ogre::Real xdelta, Ogre::Real ydelta, Ogre::Real zdelta, bool jump);

protected:
//! show joint visual debugging data
/*!
implement its own version of visual debugging
*/
virtual void showDebugData(Ogre::SceneNode* debugRootNode);


struct DebugInfo
{
void* m_playershape;
Ogre::SceneNode* m_node;
Ogre::ManualObject* m_visualDebug;
};
std::vector<DebugInfo> m_debugInfo;

};

} // end NAMESPACE OgreNewt


#endif // _INCLUDE_OGRENEWT_PLAYERCONTROLLER


OgreNewt_PlayerController.cpp

#include "OgreNewt_stdafx.h"
#include "OgreNewt_Body.h"
#include "OgreNewt_World.h"
#include "OgreNewt_Tools.h"
#include "OgreNewt_CollisionPrimitives.h"
#include "OgreNewt_PlayerController.h"
#include "CustomPlayerController.h"


namespace OgreNewt
{

PlayerController::PlayerController(OgreNewt::Body * child, Ogre::Real stairHeight, Ogre::Real maxSlope, Ogre::Real kinematicCushion)
:Joint()
{
dMatrix globalFrame (GetIdentityMatrix());
globalFrame.m_front = dVector (0.0f, 1.0f, 0.0f, 0.0f); // up direction in global Space
globalFrame.m_up = dVector (0.0f, 0.0f, -1.0f, 0.0f); // ogre front dir is the -z direction
globalFrame.m_right = globalFrame.m_front * globalFrame.m_up; // strafing direction in global Space

// create a newton player controller
CustomPlayerController* controller;
controller = new CustomPlayerController (globalFrame, child->getNewtonBody(), stairHeight, kinematicCushion);
controller->SetMaxSlope(Ogre::Degree(maxSlope).valueRadians());
SetSupportJoint(controller);
}

PlayerController::~PlayerController()
{

for (std::vector<DebugInfo>::iterator it (m_debugInfo.begin()); it != m_debugInfo.end(); it++) {
DebugInfo& info = (*it);

if (info.m_node->getParent()) {
info.m_node->getParent()->removeChild(info.m_node);
}
info.m_node->detachAllObjects();

// delete info.m_node;
delete info.m_visualDebug;

info.m_node = NULL;
info.m_visualDebug = NULL;
}
m_debugInfo.clear();
}

void PlayerController::getVelocity(Ogre::Real& forwardSpeed, Ogre::Real& sideSpeed, Ogre::Radian& heading) const
{
dFloat dir;

dir = 0.0f;
sideSpeed = 0.0f;
forwardSpeed = 0.0f;
CustomPlayerController* joint = (CustomPlayerController*) m_joint;
if (joint) {
joint->GetVelocity (forwardSpeed, sideSpeed, dir);
}

heading = dir;
}

void PlayerController::setVelocity(Ogre::Real forwardSpeed, Ogre::Real sideSpeed, Ogre::Radian heading)
{
CustomPlayerController* joint = (CustomPlayerController*) m_joint;
if (joint) {
joint->SetVelocity (forwardSpeed, sideSpeed, heading.valueRadians());
}
}

//! get the Paley height
Ogre::Real PlayerController::getPlayerHeight() const
{
Ogre::Real height = 0.0f;
CustomPlayerController* joint = (CustomPlayerController*) m_joint;
if (joint) {
height = joint->GetPlayerHeight();
}
return height;
}

void PlayerController::setStairHeight(Ogre::Real height)
{
CustomPlayerController* joint = (CustomPlayerController*) m_joint;
if (joint) {
joint->SetPlayerStairHeight(height);
}
}

void PlayerController::setPlayerState(Ogre::uint32 state)
{
CustomPlayerController* joint = (CustomPlayerController*) m_joint;
if (joint) {
joint->SetPlayerState((CustomPlayerController::PlayerState)state);
}
}

Ogre::uint32 PlayerController::getPlayerState()
{
CustomPlayerController* joint = (CustomPlayerController*) m_joint;
if (joint) {
return (Ogre::uint32)joint->GetPlayerState();
}
return 0;
}

void PlayerController::addImpulse(Ogre::Real xdelta, Ogre::Real ydelta, Ogre::Real zdelta, bool jump)
{
CustomPlayerController* joint = (CustomPlayerController*) m_joint;
if (joint) {
joint->AddImpulse(xdelta, ydelta, zdelta, jump);
}
}

void PlayerController::showDebugData(Ogre::SceneNode* debugRootNode)
{
Body* playerBody = getBody0();
CustomPlayerController* joint = (CustomPlayerController*) m_joint;

Ogre::Vector3 pos;
Ogre::Quaternion orient;

// playerBody->getPositionOrientation(pos, orient);
playerBody->getVisualPositionOrientation (pos, orient);
dMatrix matrix (dQuaternion (orient.w, orient.x, orient.y, orient.z), dVector (pos.x, pos.y, pos.z, 1.0f));

if (!m_debugInfo.size()) {
DebugInfo newInfo;
std::ostringstream oss;
Ogre::Vector3 pos;
Ogre::Quaternion orient;

OgreNewt::Debugger& debug(playerBody->getWorld()->getDebugger());

oss << "__OgreNewt__Debugger__Lines__" << joint->GetSensorShape() << "__";
newInfo.m_visualDebug = new Ogre::ManualObject(oss.str());
newInfo.m_node = debugRootNode->createChildSceneNode();
newInfo.m_playershape = (void*) joint->GetSensorShape();

OgreNewt::Collision sensorCollision (joint->GetSensorShape(), playerBody->getWorld());
debug.buildDebugObjectFromCollision(newInfo.m_visualDebug, Ogre::ColourValue(1, 0, 0, 1), sensorCollision);

newInfo.m_node->attachObject(newInfo.m_visualDebug);

// append this debug info to the array
m_debugInfo.push_back(newInfo);


oss << "__OgreNewt__Debugger__Lines__" << joint->GetStairStepShape() << "__";
newInfo.m_visualDebug = new Ogre::ManualObject(oss.str());
newInfo.m_node = debugRootNode->createChildSceneNode();
newInfo.m_playershape = (void*) joint->GetSensorShape();

OgreNewt::Collision stairCollision (joint->GetStairStepShape(), playerBody->getWorld());
debug.buildDebugObjectFromCollision(newInfo.m_visualDebug, Ogre::ColourValue(1, 1, 0, 1), stairCollision);

newInfo.m_node->attachObject(newInfo.m_visualDebug);

// append this debug info to the array
m_debugInfo.push_back(newInfo);
}


DebugInfo& shapeInfo = m_debugInfo[0];
shapeInfo.m_node->setPosition(pos);
shapeInfo.m_node->setOrientation (orient);
if (!shapeInfo.m_node->getParent()) {
debugRootNode->addChild(shapeInfo.m_node);
}

pos.y += joint->GetPlayerStairHeight();
DebugInfo& stairInfo = m_debugInfo[1];
stairInfo.m_node->setPosition(pos);
stairInfo.m_node->setOrientation (orient);
if (!stairInfo.m_node->getParent()) {
debugRootNode->addChild(stairInfo.m_node);
}
}

} // end NAMESPACE OgreNewt


Note that this does NOT fix the other issues with stair stepping, it just fixes the jumping.

- Houdini

atm, I'm deleting the player controller pointer and making a new one when trying to jump.
This works with walking on stairs and jumping, but it also resets some heading stuff. and changes the direction I walk in...
Saving the heading and reloading doesn't solve this..

So does your solution allow walking up stairs? otherwise i'll just keep on trying with my solution

Yes, it works with walking up stairs. As you know, jumping doesn't work with the stair factor turned on. So basically, when you execute the function to jump it just sets a flag in the Newton CustomPlayerContoller to IGNORE stair step for a single frame. This allows the player to get off the ground, at which point the flag is immediately reset and stair stepping is used again.

Just a easier way than trying to manually turn on/off stair stepping each time you want the player to jump.

- Houdini

sounds good, i'll try it out tomorrow and let you know how it works out

i dont know where the problem is, but houdini's code broke my ogrenewt/newton. I had to go back to my backup again..

does anyone maybe know this? atm i just delete the playercontroller pointer and make a new one with low stairheight when jumping, and doing the same to reset to normal stair heigt.
This works ok, but somehow the heading is reset. I've tried saving heading and putting it back in after recreation but this does not work.
Any ideas?

if(mKeyboard->isKeyDown(OIS::KC_SPACE))
{
delete mPlayer;
mPlayer = new OgreNewt::PlayerController(playerBody, 0.0001);

playerBody->addImpulse(playerBody->getOrientation()* Ogre::Vector3(0, 1, 0), playerBody->getPosition());
jump = true;
}

i've got a solution for this, although its ugly as hell, it works, and because of lack of time i'll use this for now:

Ogre::Quaternion q = playerBody->getOrientation();
playerBody->setPositionOrientation(playerBody->getPosition(), Ogre::Quaternion::IDENTITY);
delete mPlayer;
mPlayer = new OgreNewt::PlayerController(playerBody, 0.4);
playerBody->setPositionOrientation(playerBody->getPosition(),q);
playerBody->addImpulse(playerBody->getOrientation()* Ogre::Vector3(0, 1, 0), playerBody->getPosition());
jump = true;


now i'm going to work on collision detection, i hope i'll get less trouble for that!