float AI_Car_Experimental::RayCastDistance( MATHVECTOR <float, 3> direction, float max_length){
btVector3 pos = car->GetCarDynamics().GetPosition();
btVector3 dir = car->GetCarDynamics().LocalToWorld(ToBulletVector(direction));
dir -= pos;
COLLISION_CONTACT contact;
car->GetDynamicsWorld()->castRay(
pos,
dir,
max_length,
&car->GetCarDynamics().getCollisionObject(),
contact
);
float depth = contact.GetDepth();
float dist = std::min(max_length, depth);
#ifdef VISUALIZE_AI_DEBUG
MATHVECTOR<float, 3> pos_start(ToMathVector<float>(pos));
MATHVECTOR<float, 3> pos_end = pos_start + (ToMathVector<float>(dir) * dist);
AddLinePoint(raycastshape, pos_start);
AddLinePoint(raycastshape, pos_end);
#endif
return dist;
}
void FollowCamera::update(Real time, const PosInfo& posIn, PosInfo* posOut, COLLISION_WORLD* world, bool bounce)
{
if (!ca || !posOut) return;
/// input from car posInfoIn
Vector3 posGoal = posIn.pos;
Quaternion orientGoal = posIn.rot;
/// output saved back to car posInfoOut
Quaternion camRotFinal;
const static Quaternion
qO = Quaternion(Degree(180),Vector3::UNIT_Z) * Quaternion(Degree(-90),Vector3::UNIT_Y),
qR = Quaternion(Degree(90),Vector3(0,1,0));
Quaternion orient = orientGoal * qO;
Vector3 ofs = orient * ca->mOffset, goalLook = posGoal + ofs;
first = iFirst < 2; ///par few first frames after reset
if (iFirst < 10) // after reset
{
++iFirst;
mDistReduce = 0.f;
mATilt = 0.f;
}
/// Camera Tilt from terrain/road slope under car
//-------------------------------------------------------------------------------------------
const float // params . . .
Rdist = 1.f, // dist from car to ray (front or back)
HupCar = 1.5f, // car up dir dist - for pipes - so pos stays inside pipe
Habove = 1.5f, // up axis dist, above car - for very high terrain angles
HMaxDepth = 12.f; // how far down can the ray goes
const static Radian r0(0.f),
angMin = Degree(10.f), // below this angle, tilt has no effect - terrain bumps
maxDiff = Degree(1.4f); // max diff of tilt - no sudden jumps
const float smoothSpeed = 14.f; // how fast to apply tilt change
bool bUseTilt = ca->mType == CAM_ExtAng || ca->mType == CAM_Follow;
Radian tilt(0.f);
if (pSet->cam_tilt && bUseTilt)
{
// car pos
Vector3 carUp = posIn.pos - HupCar * posIn.carY;
MATHVECTOR<float,3> pos(carUp.x, -carUp.z, carUp.y + Habove); // to vdr/blt
const static MATHVECTOR<float,3> dir(0,0,-1); // cast rays down
// car rot, yaw angle
Quaternion q = posIn.rot * Quaternion(Degree(90),Vector3(0,1,0));
float angCarY = q.getYaw().valueRadians() + PI_d/2.f;
float ax = cosf(angCarY)*Rdist, ay = sinf(angCarY)*Rdist;
//LogO("pos: "+fToStr(pos[0],2,4)+" "+fToStr(pos[1],2,4)+" a: "+fToStr(angCarY,2,4)+" dir: "+fToStr(ax,2,4)+" "+fToStr(ay,2,4));
// cast 2 rays - 2 times, average 2 angles
COLLISION_CONTACT ct0,ct1,ct20,ct21;
MATHVECTOR<float,3> ofs(ax*0.5f,ay*0.5f,0),ofs2(ax,ay,0);
world->CastRay(pos+ofs, dir, HMaxDepth,chassis, ct0, 0,0, true, true);
world->CastRay(pos-ofs, dir, HMaxDepth,chassis, ct1, 0,0, true, true);
world->CastRay(pos+ofs2,dir, HMaxDepth,chassis, ct20, 0,0, true, true);
world->CastRay(pos-ofs2,dir, HMaxDepth,chassis, ct21, 0,0, true, true);
#ifdef CAM_TILT_DBG
MATHVECTOR<float,3> v;
v = pos+ofs;
posHit[0] = Vector3(v[0],v[2]- ct0.GetDepth(), -v[1]);
v = pos-ofs;
posHit[1] = Vector3(v[0],v[2]- ct1.GetDepth(), -v[1]);
v = pos+ofs2;
posHit[2] = Vector3(v[0],v[2]- ct20.GetDepth(),-v[1]);
v = pos-ofs2;
posHit[3] = Vector3(v[0],v[2]- ct21.GetDepth(),-v[1]);
#endif
if (ct0.GetColObj() && ct1.GetColObj() && ct20.GetColObj() && ct21.GetColObj() )
tilt = (GetAngle(Rdist, ct1.GetDepth() - ct0.GetDepth()) +
GetAngle(2.f*Rdist, ct21.GetDepth() - ct20.GetDepth())) * 0.5f;
//else LogO(String("no hit: ")+(ct0.col?"1":"0")+(ct1.col?" 1":" 0"));
//if (tilt < angMin && tilt > -angMin) tilt = 0.f;
if (tilt < r0 && tilt >-angMin) {
Radian d = tilt-angMin;
tilt = std::min(r0, tilt + d*d*5.f);
}
if (tilt > r0 && tilt < angMin) {
Radian d =-angMin-tilt;
tilt = std::max(r0, tilt - d*d*5.f);
}
//LogO("a "+fToStr(angCarY,3,5)+" d "+fToStr(ct0.GetDepth(),3,5)+" "+fToStr(ct1.GetDepth(),3,5)+" t "+fToStr(tilt.valueDegrees(),3,5));
}
// smooth tilt angle
mATilt += std::min(maxDiff, std::max(-maxDiff, tilt - mATilt)) * time * smoothSpeed;
//-------------------------------------------------------------------------------------------
if (ca->mType == CAM_Car) /* 3 Car - car pos & rot full */
{
camPosFinal = goalLook;
camRotFinal = orient;
posOut->camPos = camPosFinal; // save result in out posInfo
posOut->camRot = camRotFinal;
return;
}
if (ca->mType == CAM_Follow) ofs = ca->mOffset;
Vector3 pos,goalPos;
pos = camPosFinal - ofs;
goalPos = posGoal;
Vector3 xyz;
if (ca->mType != CAM_Arena)
{
Real x,y,z,xz; // pitch & yaw to direction vector
Real ap = bUseTilt ? (ca->mPitch.valueRadians() + mATilt.valueRadians()) : ca->mPitch.valueRadians(),
ay = ca->mYaw.valueRadians();
y = sin(ap), xz = cos(ap);
x = sin(ay) * xz, z = cos(ay) * xz;
xyz = Vector3(x,y,z);
xyz *= ca->mDist;
}
bool manualOrient = false;
switch (ca->mType)
{
case CAM_Arena: /* 2 Arena - free pos & rot */
goalPos = ca->mOffset - ofs;
break;
case CAM_Free: /* 1 Free - free rot, pos from car */
goalPos += xyz;
break;
case CAM_Follow: /* 0 Follow - car rotY & pos from behind car, smooth */
{ Quaternion orient = orientGoal * qR;
orient.FromAngleAxis(orient.getYaw(), Vector3::UNIT_Y);
goalPos += orient * xyz;
}
break;
case CAM_ExtAng: /* 4 Extended Angle - car in center, angle smooth */
{ Quaternion orient = orientGoal * qR;
Quaternion ory;
ory.FromAngleAxis(orient.getYaw(), Vector3::UNIT_Y);
if (first) {
qq = ory;
}
else qq = orient.Slerp(ca->mSpeed * time, qq, ory, true);
// smooth dist from vel
#if 0
{
if (first) {
mPosNodeOld = posGoal;
}
Real vel = (posGoal - mPosNodeOld).length() / std::max(0.002f, std::min(0.1f, time));
mPosNodeOld = posGoal;
if (first) mVel = 0.f;
else
mVel += (vel - mVel) * time * 8.f; //par- vel smooth speed
if (!first)
xyz *= 1.f + std::min(100.f, mVel) * 0.01f; //par- vel dist factor
}
#endif
Quaternion qy = Quaternion(ca->mYaw,Vector3(0,1,0));
goalPos += qq * (xyz + ca->mOffset);
camPosFinal = goalPos;
camRotFinal = qq * qy * Quaternion(Degree(-ca->mPitch - mATilt), Vector3(1,0,0));
manualOrient = true;
}
break;
}
if (!manualOrient) // if !CAM_ExtAng
{
float dtmul = ca->mSpeed == 0 ? 1.0f : ca->mSpeed * time;
if (ca->mType == CAM_Arena)
{
Vector3 Pos(0,0,0), goalPos = ca->mOffset;
Pos = camPosFinal; //read last state (smooth)
Pos += (goalPos - Pos) * dtmul;
mAPitch += (ca->mPitch - mAPitch) * dtmul;
mAYaw += (ca->mYaw - mAYaw) * dtmul;
if (first) {
Pos = goalPos;
mAPitch = ca->mPitch;
mAYaw = ca->mYaw;
}
camPosFinal = Pos;
camRotFinal = Quaternion(Degree(mAYaw),Vector3(0,1,0)) * Quaternion(Degree(mAPitch),Vector3(1,0,0));
manualOrient = true;
}
else
{
if (first) pos = goalPos;
Vector3 addPos,addLook;
addPos = (goalPos - pos).normalisedCopy() * (goalPos - pos).length() * dtmul;
if (addPos.squaredLength() > (goalPos - pos).squaredLength()) addPos = goalPos - pos;
pos += addPos;
camPosFinal = pos + ofs;
goalLook = posGoal + ofs;
if (first) {
mLook = goalLook;
}
addLook = (goalLook - mLook) * dtmul;//Rot;
mLook += addLook;
}
}
//camLookFinal = mLook;
if (!manualOrient) // CAM_Free or CAM_Follow
{
Vector3 zdir = camPosFinal - mLook;
zdir.normalise();
Vector3 xVec = Vector3::UNIT_Y.crossProduct(zdir);
xVec.normalise();
Vector3 yVec = zdir.crossProduct(xVec);
yVec.normalise();
Quaternion q;
q.FromAxes(xVec, yVec, zdir);
camRotFinal = q;
}
// cast ray from car to camera, reduce dist if hit
//-------------------------------------------------------------------------------------------
Vector3 pp = camPosFinal;
if (bounce)
pp += posIn.camOfs * ca->mOfsMul
* gPar.camBncScale * pSet->cam_bnc_mul;
Vector3 p = posGoal;
p.y += 1.f; //up
//Vector3 d = camRotFinal * Vector3::UNIT_Z; d.normalise();
Vector3 d = pp - p;
d.normalise();
if (!first && ca->mType != CAM_Arena)
{
MATHVECTOR<float,3> pos1(p.x,-p.z,p.y), dir(d.x,-d.z,d.y); //dir = dir.Normalize();
COLLISION_CONTACT ct;
float maxLen = (p - pp).length(); //cam near
world->CastRay(pos1, dir, maxLen,chassis, ct, 0,0, true, true, true/*+*/);
//dbgLen = -maxLen;
if (ct.GetColObj())
{
float len = ct.GetDepth(); //dbgLen = len;
len -= 0.2f + ct.GetNormal()[2]; ///par normal up, flat terrain, move closer
if (len < maxLen)
{
Real dist = maxLen - len;
if (dist > mDistReduce)
mDistReduce = dist;
}
}
}
// save result in out posInfo
posOut->camPos = mDistReduce > 0.0001f ? (pp - d * mDistReduce) : pp;
posOut->camRot = camRotFinal;
// smooth, back to normal dist
if (mDistReduce > 0.f)
mDistReduce -= time * 10.f;
}