//////////////////////////////////////////////////////////////////////////
// Position the body precisely
//////////////////////////////////////////////////////////////////////////
void DropDownBox(NewtonBody *body)
{
float matrix[16];
NewtonBodyGetMatrix(body, matrix);
Matrix4 m = Matrix4(matrix);
Vector3 pos = m.GetPosition();
pos.y += 1.0f;
m.SetPosition(pos);
Vector3 p = pos;
p.y -= 20;
// cast collision shape within +20 -20 y range
NewtonWorld *world = NewtonBodyGetWorld(body);
NewtonCollision *collision = NewtonBodyGetCollision(body);
float param;
NewtonWorldConvexCastReturnInfo info[16];
m.FlattenToArray(matrix);
NewtonWorldConvexCast(world, matrix, &p[0], collision, ¶m, body, DropDownConvexCastCallback, info, 16, 0);
m = Matrix4(matrix);
pos = m.GetPosition();
m.SetPosition(pos + Vector3(0, (p.y - pos.y) * param, 0) );
m.FlattenToArray(matrix);
NewtonBodySetMatrix(body, matrix);
}
virtual void PreUpdate (dFloat timestep)
{
(void)timestep;
NewtonWorld* const world = GetWorld();
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);
NewtonDemos* const mainWindow = scene->GetRootWindow();
DemoCamera* const camera = scene->GetCamera();
m_helpKey.UpdatePushButton (mainWindow, 'H');
if (m_selectShape.UpdateTriggerButton (mainWindow, ' ')) {
m_stupidLevel->ChangeCastingShape();
}
bool buttonState = mainWindow->GetMouseKeyState(1);
if (buttonState) {
int mouseX;
int mouseY;
buttonState = false;
mainWindow->GetMousePosition (mouseX, mouseY);
float x = dFloat (mouseX);
float y = dFloat (mouseY);
dVector p0 (camera->ScreenToWorld(dVector (x, y, 0.0f, 0.0f)));
dVector p1 (camera->ScreenToWorld(dVector (x, y, 1.0f, 0.0f)));
//p0 = dVector (-11.531384, 6.897866, -2.453451, 1.0f);
//p1 = dVector (1720.189697, -1245.047241, 58.625248, 1.0f);
// do the convex cast here
dMatrix matrix (dGetIdentityMatrix());
matrix.m_posit = p0;
dFloat param = 1.2f;
NewtonCollision* const shape = m_stupidLevel->GetCurrentShape();
//int count = NewtonWorldConvexCast (world, &matrix[0][0], &p1[0], shape, ConvexCastCallBack::Filter, &filter, ConvexCastCallBack::Prefilter, &filter.m_contacts[0], 4, 0);
NewtonWorldConvexCast (world, &matrix[0][0], &p1[0], shape, ¶m, NULL, Prefilter, NULL, 0, 0);
//dTrace(("%f, %f, %f\n", p0[0], p0[1], p0[2]));
//dTrace(("%f, %f, %f\n", p1[0], p1[1], p1[2]));
if (param < 1.0f) {
matrix.m_posit += (p1 - matrix.m_posit).Scale (param);
m_stupidLevel->SetCastEntityMatrix (scene, matrix);
}
m_stupidLevel->SetCastingLine (p0, p1);
}
}
void Convexcast::go(const OgreNewt::World* world, const OgreNewt::ConvexCollisionPtr& col, const Ogre::Vector3& startpt, const Ogre::Quaternion &colori, const Ogre::Vector3& endpt, int maxcontactscount, int threadIndex)
{
if( world->getDebugger().isRaycastRecording() )
{
world->getDebugger().addConvexRay(col, startpt, colori, endpt);
}
// reserve memory
if( mReturnInfoListSize < maxcontactscount )
{
mReturnInfoListSize = 0;
if( mReturnInfoList )
delete[] mReturnInfoList;
mReturnInfoList = new NewtonWorldConvexCastReturnInfo[maxcontactscount];
mReturnInfoListSize = maxcontactscount;
}
memset(mReturnInfoList, 0, sizeof(mReturnInfoList[0])*mReturnInfoListSize);
// perform the cast
float matrix[16];
OgreNewt::Converters::QuatPosToMatrix(colori, startpt, &matrix[0] );
mFirstContactDistance = -1;
mReturnInfoListLength =
NewtonWorldConvexCast( world->getNewtonWorld(), &matrix[0], (float*)&endpt, col->getNewtonCollision(),
&mFirstContactDistance, this, OgreNewt::Convexcast::newtonConvexcastPreFilter,
mReturnInfoList, mReturnInfoListSize, threadIndex);
if( world->getDebugger().isRaycastRecording() && world->getDebugger().isRaycastRecordingHitBodies() )
{
Body* body;
for(int i = 0; i < mReturnInfoListLength; i++)
{
body = (OgreNewt::Body*) NewtonBodyGetUserData(mReturnInfoList[i].m_hitBody);
world->getDebugger().addHitBody(body);
}
}
}
void CustomPlayerController::UpdateGroundPlane (dMatrix& matrix, const dMatrix& castMatrix, const dVector& dst, int threadIndex)
{
CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager();
NewtonWorld* const world = manager->GetWorld();
NewtonWorldConvexCastReturnInfo info;
CustomControllerConvexRayFilter filter(m_body);
dFloat param = 10.0f;
int count = NewtonWorldConvexCast (world, &castMatrix[0][0], &dst[0], m_castingShape, ¶m, &filter, CustomControllerConvexCastPreFilter::Prefilter, &info, 1, threadIndex);
m_groundPlane = dVector (0.0f);
m_groundVelocity = dVector (0.0f);
if (count && (param <= 1.0f)) {
m_isJumping = false;
dVector supportPoint (castMatrix.m_posit + (dst - castMatrix.m_posit).Scale (param));
m_groundPlane = dVector (info.m_normal[0], info.m_normal[1], info.m_normal[2], 0.0f);
m_groundPlane.m_w = - supportPoint.DotProduct3(m_groundPlane);
NewtonBodyGetPointVelocity (info.m_hitBody, &supportPoint.m_x, &m_groundVelocity[0]);
matrix.m_posit = supportPoint;
matrix.m_posit.m_w = 1.0f;
}
}
void ConvexCastPlacement (NewtonBody* body)
{
dFloat param;
dMatrix matrix;
NewtonWorld* world;
NewtonCollision* collision;
NewtonWorldConvexCastReturnInfo info[16];
NewtonBodyGetMatrix (body, &matrix[0][0]);
matrix.m_posit.m_y += 40.0f;
dVector p (matrix.m_posit);
p.m_y -= 80.0f;
world = NewtonBodyGetWorld(body);
collision = NewtonBodyGetCollision(body);
NewtonWorldConvexCast (world, &matrix[0][0], &p[0], collision, ¶m, body, ConvexCastCallback, info, 16, 0);
dAssert (param < 1.0f);
matrix.m_posit.m_y += (p.m_y - matrix.m_posit.m_y) * param;
NewtonBodySetMatrix(body, &matrix[0][0]);
}
void CustomDGRayCastCar::CalculateTireCollision (Tire& tire, const dMatrix& suspensionMatrix, int threadIndex) const
{
int floorcontact = 0;
tire.m_HitBody = NULL;
tire.m_posit = tire.m_suspensionLenght;
if ( tire.m_tireUseConvexCastMode ) {
dFloat hitParam;
NewtonWorldConvexCastReturnInfo info;
// tire.m_rayDestination = tire.m_suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -tire.m_suspensionLenght ));
dVector rayDestination (suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -tire.m_suspensionLenght)));
if ( NewtonWorldConvexCast ( m_world, &suspensionMatrix[0][0], &rayDestination[0], tire.m_shape, &hitParam, (void*)m_body0, ConvexCastPrefilter, &info, 1, threadIndex ) ){
tire.m_posit = hitParam * tire.m_suspensionLenght;
tire.m_contactPoint = info.m_point;
tire.m_contactNormal = info.m_normal;
tire.m_HitBody = (NewtonBody*)info.m_hitBody;
floorcontact = 1;
}
} else {
struct RayCastInfo
{
RayCastInfo (const NewtonBody* body)
{
m_param = 1.0f;
m_me = body;
m_hitBody = NULL;
m_contactID = 0;
m_normal = dVector (0.0f, 0.0f, 0.0f, 1.0f);
}
static dFloat RayCast (const NewtonBody* body, const dFloat* normal, int collisionID, void* userData, dFloat intersetParam)
{
RayCastInfo& caster = *( (RayCastInfo*) userData );
// if this body is not the vehicle, see if a close hit
if ( body != caster.m_me ) {
if ( intersetParam < caster.m_param) {
// this is a close hit, record the information.
caster.m_param = intersetParam;
caster.m_hitBody = body;
caster.m_contactID = collisionID;
caster.m_normal = dVector (normal[0], normal[1], normal[2], 1.0f);
}
}
return intersetParam;
}
dFloat m_param;
dVector m_normal;
const NewtonBody* m_me;
const NewtonBody* m_hitBody;
int m_contactID;
};
RayCastInfo info (m_body0);
_ASSERTE (0);
// extend the ray by the radius of the tire
dFloat dist ( tire.m_suspensionLenght + tire.m_radius );
dVector rayDestination (suspensionMatrix.TransformVector (m_localFrame.m_up.Scale ( -dist )));
// cast a ray to the world ConvexCastPrefilter
NewtonWorldRayCast( m_world, &suspensionMatrix.m_posit[0], &rayDestination[0], RayCastInfo::RayCast, &info, &ConvexCastPrefilter );
// if the ray hit something, it means the tire has some traction
if ( info.m_hitBody ) {
dFloat intesectionDist;
tire.m_HitBody = (NewtonBody*)info.m_hitBody;
tire.m_contactPoint = suspensionMatrix.m_posit + (rayDestination - suspensionMatrix.m_posit ).Scale ( info.m_param );
tire.m_contactNormal = info.m_normal;
// TO DO: get the material properties for tire frictions on different roads
intesectionDist = ( dist * info.m_param - tire.m_radius );
if ( intesectionDist < 0.0f ) {
intesectionDist = 0.0f;
} else if ( intesectionDist > tire.m_suspensionLenght ) {
intesectionDist = tire.m_suspensionLenght;
}
tire.m_posit = intesectionDist;
}
}
}
void tPlayerController::_upateState(float timeStep)
{
Vec3 nextStep = Vec3::Zero;
if (mState == eOnLand)
{
nextStep += mForwordVelocity * mNode->GetDirection();
nextStep += mSideVelocity * mNode->GetRight();
}
else if (mState == eOnFall)
{
nextStep += Vec3::UnitY * mGravity;
}
else if (mState == eOnJump)
{
nextStep += mForwordVelocity * mNode->GetDirection();
nextStep += mSideVelocity * mNode->GetRight();
nextStep += mInternalJumpVel * Vec3::UnitY;
}
nextStep *= timeStep;
const Vec3 & worldPos = mNode->GetPosition();
NewtonWorld * ntWorld = tWorld::Instance()->_getNewtonWorld();
NewtonWorldConvexCastReturnInfo info[_NT_MAX_CONTACTS];
Mat4 worldTm;
worldTm.MakeTranslate(worldPos.x, worldPos.y, worldPos.z);
Vec3 dest = worldPos + nextStep;
float hitParam;
// check on land
if (mState == eOnJump)
{
int contacts = NewtonWorldConvexCast(ntWorld, worldTm[0], &dest.x, mShape->_getNewtonCollision(), &hitParam, this,
_convexCastBodyFilter, info, _NT_MAX_CONTACTS, 0);
if (contacts)
{
float bestValue = info[0].m_point[1];
float lowestValue = info[0].m_point[1];
for (int i = 1; i < contacts; i ++)
{
float value = info[i].m_point[1];
if (value > bestValue)
bestValue = value;
if (value < lowestValue)
lowestValue = value;
}
if (mInternalJumpVel >= 0 && bestValue > mPlayerHeight * mJumpHighLimit)
mInternalJumpVel = 0;
/*if (mInternalJumpVel <= 0 && lowestValue < mPlayerHeight * mLandLimit)
{
mState = eOnLand;
}
*/
Vec3 Normal, Position;
if (mInternalJumpVel <= 0 && _checkOnLand(dest, &Normal, &Position))
{
mNode->SetPosition(Position);
mState = eOnLand;
}
}
mNode->SetPosition(dest);
return ;
}
Vec3 Normal, Position;
if (_checkOnLand(dest, &Normal, &Position))
{
int contacts = NewtonWorldConvexCast(ntWorld, worldTm[0], &dest.x, mShape->_getNewtonCollision(), &hitParam, this,
_convexCastBodyFilter, info, _NT_MAX_CONTACTS, 0);
float bestValue = info[0].m_point[1];
float lowestValue = info[0].m_point[1];
for (int i = 1; i < contacts; i ++)
{
float value = info[i].m_point[1];
if (value > bestValue)
bestValue = value;
if (value < lowestValue)
lowestValue = value;
}
if (bestValue < dest.y + mPlayerHeight * mLandLimit)
mState = eOnLand;
else
mState = eOnIlligalRamp;
/*if (Normal.y < mMaxSlopeCos)
{
mState = eOnIlligalRamp;
return ;
}*/
if (mState == eOnLand)
{
mNode->SetPosition(Position);
}
return ;
}
mNode->SetPosition(dest);
mState = eOnFall;
}
void CustomPlayerController::PostUpdate(dFloat timestep, int threadIndex)
{
dMatrix matrix;
dQuaternion bodyRotation;
dVector veloc(0.0f, 0.0f, 0.0f, 0.0f);
dVector omega(0.0f, 0.0f, 0.0f, 0.0f);
CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager();
NewtonWorld* const world = manager->GetWorld();
// apply the player motion, by calculation the desired plane linear and angular velocity
manager->ApplyPlayerMove (this, timestep);
// get the body motion state
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
NewtonBodyGetVelocity(m_body, &veloc[0]);
NewtonBodyGetOmega(m_body, &omega[0]);
// integrate body angular velocity
NewtonBodyGetRotation (m_body, &bodyRotation.m_q0);
bodyRotation = bodyRotation.IntegrateOmega(omega, timestep);
matrix = dMatrix (bodyRotation, matrix.m_posit);
// integrate linear velocity
dFloat normalizedTimeLeft = 1.0f;
dFloat step = timestep * dSqrt (veloc % veloc) ;
dFloat descreteTimeStep = timestep * (1.0f / D_DESCRETE_MOTION_STEPS);
int prevContactCount = 0;
CustomControllerConvexCastPreFilter castFilterData (m_body);
NewtonWorldConvexCastReturnInfo prevInfo[PLAYER_CONTROLLER_MAX_CONTACTS];
dVector updir (matrix.RotateVector(m_upVector));
dVector scale;
NewtonCollisionGetScale (m_upperBodyShape, &scale.m_x, &scale.m_y, &scale.m_z);
//const dFloat radio = m_outerRadio * 4.0f;
const dFloat radio = (m_outerRadio + m_restrainingDistance) * 4.0f;
NewtonCollisionSetScale (m_upperBodyShape, m_height - m_stairStep, radio, radio);
NewtonWorldConvexCastReturnInfo upConstratint;
memset (&upConstratint, 0, sizeof (upConstratint));
upConstratint.m_normal[0] = m_upVector.m_x;
upConstratint.m_normal[1] = m_upVector.m_y;
upConstratint.m_normal[2] = m_upVector.m_z;
upConstratint.m_normal[3] = m_upVector.m_w;
for (int j = 0; (j < D_PLAYER_MAX_INTERGRATION_STEPS) && (normalizedTimeLeft > 1.0e-5f); j ++ ) {
if ((veloc % veloc) < 1.0e-6f) {
break;
}
dFloat timetoImpact;
NewtonWorldConvexCastReturnInfo info[PLAYER_CONTROLLER_MAX_CONTACTS];
dVector destPosit (matrix.m_posit + veloc.Scale (timestep));
int contactCount = NewtonWorldConvexCast (world, &matrix[0][0], &destPosit[0], m_upperBodyShape, &timetoImpact, &castFilterData, CustomControllerConvexCastPreFilter::Prefilter, info, sizeof (info) / sizeof (info[0]), threadIndex);
if (contactCount) {
contactCount = manager->ProcessContacts (this, info, contactCount);
}
if (contactCount) {
matrix.m_posit += veloc.Scale (timetoImpact * timestep);
if (timetoImpact > 0.0f) {
matrix.m_posit -= veloc.Scale (D_PLAYER_CONTACT_SKIN_THICKNESS / dSqrt (veloc % veloc)) ;
}
normalizedTimeLeft -= timetoImpact;
dFloat speed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
dFloat bounceSpeed[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
dVector bounceNormal[PLAYER_CONTROLLER_MAX_CONTACTS * 2];
for (int i = 1; i < contactCount; i ++) {
dVector n0 (info[i-1].m_normal);
for (int j = 0; j < i; j ++) {
dVector n1 (info[j].m_normal);
if ((n0 % n1) > 0.9999f) {
info[i] = info[contactCount - 1];
i --;
contactCount --;
break;
}
}
}
int count = 0;
if (!m_isJumping) {
upConstratint.m_point[0] = matrix.m_posit.m_x;
upConstratint.m_point[1] = matrix.m_posit.m_y;
upConstratint.m_point[2] = matrix.m_posit.m_z;
upConstratint.m_point[3] = matrix.m_posit.m_w;
speed[count] = 0.0f;
bounceNormal[count] = dVector (upConstratint.m_normal);
bounceSpeed[count] = CalculateContactKinematics(veloc, &upConstratint);
count ++;
}
for (int i = 0; i < contactCount; i ++) {
speed[count] = 0.0f;
bounceNormal[count] = dVector (info[i].m_normal);
bounceSpeed[count] = CalculateContactKinematics(veloc, &info[i]);
count ++;
}
for (int i = 0; i < prevContactCount; i ++) {
speed[count] = 0.0f;
bounceNormal[count] = dVector (prevInfo[i].m_normal);
bounceSpeed[count] = CalculateContactKinematics(veloc, &prevInfo[i]);
count ++;
}
dFloat residual = 10.0f;
dVector auxBounceVeloc (0.0f, 0.0f, 0.0f, 0.0f);
for (int i = 0; (i < D_PLAYER_MAX_SOLVER_ITERATIONS) && (residual > 1.0e-3f); i ++) {
residual = 0.0f;
for (int k = 0; k < count; k ++) {
dVector normal (bounceNormal[k]);
dFloat v = bounceSpeed[k] - normal % auxBounceVeloc;
dFloat x = speed[k] + v;
if (x < 0.0f) {
v = 0.0f;
x = 0.0f;
}
if (dAbs (v) > residual) {
residual = dAbs (v);
}
auxBounceVeloc += normal.Scale (x - speed[k]);
speed[k] = x;
}
}
dVector velocStep (0.0f, 0.0f, 0.0f, 0.0f);
for (int i = 0; i < count; i ++) {
dVector normal (bounceNormal[i]);
velocStep += normal.Scale (speed[i]);
}
veloc += velocStep;
dFloat velocMag2 = velocStep % velocStep;
if (velocMag2 < 1.0e-6f) {
dFloat advanceTime = dMin (descreteTimeStep, normalizedTimeLeft * timestep);
matrix.m_posit += veloc.Scale (advanceTime);
normalizedTimeLeft -= advanceTime / timestep;
}
prevContactCount = contactCount;
memcpy (prevInfo, info, prevContactCount * sizeof (NewtonWorldConvexCastReturnInfo));
} else {
matrix.m_posit = destPosit;
matrix.m_posit.m_w = 1.0f;
break;
}
}
NewtonCollisionSetScale (m_upperBodyShape, scale.m_x, scale.m_y, scale.m_z);
// determine if player is standing on some plane
dMatrix supportMatrix (matrix);
supportMatrix.m_posit += updir.Scale (m_sphereCastOrigin);
if (m_isJumping) {
dVector dst (matrix.m_posit);
UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
} else {
step = dAbs (updir % veloc.Scale (timestep));
dFloat castDist = ((m_groundPlane % m_groundPlane) > 0.0f) ? m_stairStep : step;
dVector dst (matrix.m_posit - updir.Scale (castDist * 2.0f));
UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex);
}
// set player velocity, position and orientation
NewtonBodySetVelocity(m_body, &veloc[0]);
NewtonBodySetMatrix (m_body, &matrix[0][0]);
}