btScalar Tire::PacejkaSvy(
btScalar sigma,
btScalar alpha,
btScalar gamma,
btScalar dFz,
btScalar Dy)
{
const btScalar * p = coefficients;
btScalar Dv = Dy * (p[RVY1] + p[RVY2] * dFz + p[RVY3] * gamma) * btCos(btAtan(p[RVY4] * alpha));
btScalar Sv = Dv * btSin(p[RVY5] * btAtan(p[RVY6] * sigma));
return Sv;
}
btScalar Tire::PacejkaGy(
btScalar sigma,
btScalar alpha)
{
const btScalar * p = coefficients;
btScalar B = p[RBY1] * btCos(btAtan(p[RBY2] * (alpha - p[RBY3])));
btScalar C = p[RCY1];
btScalar Sh = p[RHY1];
btScalar S = sigma + Sh;
btScalar G0 = btCos(C * btAtan(B * Sh));
btScalar G = btCos(C * btAtan(B * S)) / G0;
return G;
}
btScalar Tire::PacejkaGx(
btScalar sigma,
btScalar alpha)
{
const btScalar * p = coefficients;
btScalar B = p[RBX1] * btCos(btAtan(p[RBX2] * sigma));
btScalar C = p[RCX1];
btScalar Sh = p[RHX1];
btScalar S = alpha + Sh;
btScalar G0 = btCos(C * btAtan(B * Sh));
btScalar G = btCos(C * btAtan(B * S)) / G0;
return G;
}
btScalar Tire::PacejkaFy(
btScalar alpha,
btScalar gamma,
btScalar Fz,
btScalar dFz,
btScalar friction_coeff,
btScalar & Dy,
btScalar & BCy,
btScalar & Shf) const
{
const btScalar * p = coefficients;
btScalar Fz0 = nominal_load;
// vertical shift
btScalar Sv = Fz * (p[PVY1] + p[PVY2] * dFz + (p[PVY3] + p[PVY4] * dFz) * gamma);
// horizontal shift
btScalar Sh = p[PHY1] + p[PHY2] * dFz + p[PHY3] * gamma;
// composite slip angle
btScalar A = alpha + Sh;
// slope at origin
btScalar K = p[PKY1] * Fz0 * btSin(2 * btAtan(Fz / (p[PKY2] * Fz0))) * (1 - p[PKY3] * btFabs(gamma));
// curvature factor
btScalar E = (p[PEY1] + p[PEY2] * dFz) * (1 - (p[PEY3] + p[PEY4] * gamma) * sgn(A));
// peak factor
btScalar D = Fz * (p[PDY1] + p[PDY2] * dFz) * (1 - p[PDY3] * gamma * gamma);
// shape factor
btScalar C = p[PCY1];
// stiffness factor
btScalar B = K / (C * D);
// force
btScalar F = D * btSin(C * btAtan(B * A - E * (B * A - btAtan(B * A)))) + Sv;
// scale by surface friction
F *= friction_coeff;
// aligning torque params
Dy = D;
BCy = B * C;
Shf = Sh + Sv / K;
return F;
}
btScalar Tire::PacejkaMz(
btScalar alpha,
btScalar gamma,
btScalar Fz,
btScalar dFz,
btScalar friction_coeff,
btScalar Fy,
btScalar BCy,
btScalar Shf) const
{
const btScalar * p = coefficients;
btScalar Fz0 = nominal_load;
btScalar R0 = 0.3;
btScalar yz = gamma;
btScalar cos_alpha = btCos(alpha);
btScalar Sht = p[QHZ1] + p[QHZ2] * dFz + (p[QHZ3] + p[QHZ4] * dFz) * yz;
btScalar At = alpha + Sht;
btScalar Bt = (p[QBZ1] + p[QBZ2] * dFz + p[QBZ3] * dFz * dFz) * (1 + p[QBZ4] * yz + p[QBZ5] * btFabs(yz));
btScalar Ct = p[QCZ1];
btScalar Dt = Fz * (p[QDZ1] + p[QDZ2] * dFz) * (1 + p[QDZ3] * yz + p[QDZ4] * yz * yz) * (R0 / Fz0);
btScalar Et = (p[QEZ1] + p[QEZ2] * dFz + p[QEZ3] * dFz * dFz) * (1 + (p[QEZ4] + p[QEZ5] * yz) * btAtan(Bt * Ct * At));
btScalar Mzt = -Fy * Dt * btCos(Ct * btAtan(Bt * At - Et * (Bt * At - btAtan(Bt * At)))) * cos_alpha;
btScalar Ar = alpha + Shf;
btScalar Br = p[QBZ10] * BCy;
btScalar Dr = Fz * (p[QDZ6] + p[QDZ7] * dFz + (p[QDZ8] + p[QDZ9] * dFz) * yz) * R0;
btScalar Mzr = Dr * btCos(btAtan(Br * Ar)) * cos_alpha * friction_coeff;
return Mzt + Mzr;
}
btScalar Tire::PacejkaFx(
btScalar sigma,
btScalar Fz,
btScalar dFz,
btScalar friction_coeff) const
{
const btScalar * p = coefficients;
// vertical shift
btScalar Sv = Fz * (p[PVX1] + p[PVX2] * dFz);
// horizontal shift
btScalar Sh = p[PHX1] + p[PHX2] * dFz;
// composite slip
btScalar S = sigma + Sh;
// slope at origin
btScalar K = Fz * (p[PKX1] + p[PKX2] * dFz) * btExp(-p[PKX3] * dFz);
// curvature factor
btScalar E = (p[PEX1] + p[PEX2] * dFz + p[PEX3] * dFz * dFz) * (1 - p[PEX4] * sgn(S));
// peak factor
btScalar D = Fz * (p[PDX1] + p[PDX2] * dFz);
// shape factor
btScalar C = p[PCX1];
// stiffness factor
btScalar B = K / (C * D);
// force
btScalar F = D * btSin(C * btAtan(B * S - E * (B * S - btAtan(B * S)))) + Sv;
// scale by surface friction
F *= friction_coeff;
return F;
}
/* -----------------------------------------------------------------------
| the function describe line segment :
| - rayFrom : camera position
| - rayTo : a point from mouse point on near plane raycasting to far plane
|
| only for perspective, not ortho viewport
| current method reference from DemoApplication::getRayTo
----------------------------------------------------------------------- */
bool
makeRayCastingSegment(float mouse_x, float mouse_y, Ogre::Camera* cam, btVector3& rayFrom, btVector3& rayTo)
{
float nearPlane = 1.0f;
// float nearPlane = cam->getNearClipDistance();
float farPlane = std::max(cam->getFarClipDistance(), 100.0f);
// float fovy = cam->getFOVy().valueDegrees();
float top = 1.f;
float bottom = -1.f;
float tanFov = (top-bottom) * 0.5f / nearPlane;
float fov = btScalar(2.0) * btAtan(tanFov);
float aspect = cam->getAspectRatio();
float screenWidth = (float)(cam->getViewport()->getActualWidth());
float screenHeight = (float)(cam->getViewport()->getActualHeight());
if (Ogre::Math::isNaN(aspect))
{
OGRE_EXCEPT(Ogre::Exception::ERR_NOT_IMPLEMENTED,
"Calculate aspect is NAN.",
"::makeRayCastingResult");
return false;
} // End if
rayFrom = ConvertOgreVectorTobtVector(cam->getPosition());
btVector3 rayForward = ConvertOgreVectorTobtVector(cam->getDirection().normalisedCopy());
rayForward *= farPlane;
btVector3 vertical = ConvertOgreVectorTobtVector(cam->getUp());
btVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f*fov);
hor *= 2.0f * farPlane * tanfov;
vertical *= 2.0f * farPlane * tanfov;
hor *= aspect;
btVector3 rayToCenter = rayFrom + rayForward;
btVector3 dHor = hor * 1.0f/screenWidth;
btVector3 dVert = vertical * 1.0f/screenHeight;
rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += btScalar(mouse_x) * dHor;
rayTo -= btScalar(mouse_y) * dVert;
return true;
} //End for makeRayCastingResult
btVector3 BulletOpenGLApplication::GetPickingRay(int x, int y) {
// calculate the field-of-view
float tanFov = 1.0f / m_nearPlane;
float fov = btScalar(2.0) * btAtan(tanFov);
// get a ray pointing forward from the
// camera and extend it to the far plane
btVector3 rayFrom = m_cameraPosition;
btVector3 rayForward = (m_cameraTarget - m_cameraPosition);
rayForward.normalize();
rayForward*= m_farPlane;
// find the horizontal and vertical vectors
// relative to the current camera view
btVector3 ver = m_upVector;
btVector3 hor = rayForward.cross(ver);
hor.normalize();
ver = hor.cross(rayForward);
ver.normalize();
hor *= 2.f * m_farPlane * tanFov;
ver *= 2.f * m_farPlane * tanFov;
// calculate the aspect ratio
btScalar aspect = m_screenWidth / (btScalar)m_screenHeight;
// adjust the forward-ray based on
// the X/Y coordinates that were clicked
hor*=aspect;
btVector3 rayToCenter = rayFrom + rayForward;
btVector3 dHor = hor * 1.f/float(m_screenWidth);
btVector3 dVert = ver * 1.f/float(m_screenHeight);
btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * ver;
rayTo += btScalar(x) * dHor;
rayTo -= btScalar(y) * dVert;
// return the final result
return rayTo;
}
btVector3 getRayTo(float x, float y, int heightPixels, int widthPixels)
{
m_glutScreenWidth = widthPixels;
m_glutScreenHeight = heightPixels;
if (m_ortho)
{
btScalar aspect;
btVector3 extents;
aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
extents.setValue(aspect * 1.0f, 1.0f,0);
extents *= m_cameraDistance;
btVector3 lower = m_cameraTargetPosition - extents;
btVector3 upper = m_cameraTargetPosition + extents;
btScalar u = x / btScalar(m_glutScreenWidth);
btScalar v = (m_glutScreenHeight - y) / btScalar(m_glutScreenHeight);
btVector3 p(0,0,0);
p.setValue((1.0f - u) * lower.getX() + u * upper.getX(),(1.0f - v) * lower.getY() + v * upper.getY(),m_cameraTargetPosition.getZ());
return p;
}
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = btScalar(2.0) * btAtan(tanFov);
btVector3 rayFrom = getCameraPosition();
btVector3 rayForward = (getCameraTargetPosition()-getCameraPosition());
rayForward.normalize();
float farPlane = 10000.f;
rayForward*= farPlane;
btVector3 rightOffset;
btVector3 vertical = m_cameraUp;
btVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
btScalar aspect;
aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight;
hor*=aspect;
btVector3 rayToCenter = rayFrom + rayForward;
btVector3 dHor = hor * 1.f/float(m_glutScreenWidth);
btVector3 dVert = vertical * 1.f/float(m_glutScreenHeight);
btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += btScalar(x) * dHor;
rayTo -= btScalar(y) * dVert;
//rayTo = rayTo / 100.f;
return rayTo;
}
btVector3 PhysicsServerExample::getRayTo(int x,int y)
{
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (!renderer)
{
btAssert(0);
return btVector3(0,0,0);
}
float top = 1.f;
float bottom = -1.f;
float nearPlane = 1.f;
float tanFov = (top-bottom)*0.5f / nearPlane;
float fov = btScalar(2.0) * btAtan(tanFov);
btVector3 camPos,camTarget;
renderer->getActiveCamera()->getCameraPosition(camPos);
renderer->getActiveCamera()->getCameraTargetPosition(camTarget);
btVector3 rayFrom = camPos;
btVector3 rayForward = (camTarget-camPos);
rayForward.normalize();
float farPlane = 10000.f;
rayForward*= farPlane;
btVector3 rightOffset;
btVector3 cameraUp=btVector3(0,0,0);
cameraUp[m_guiHelper->getAppInterface()->getUpAxis()]=1;
btVector3 vertical = cameraUp;
btVector3 hor;
hor = rayForward.cross(vertical);
hor.normalize();
vertical = hor.cross(rayForward);
vertical.normalize();
float tanfov = tanf(0.5f*fov);
hor *= 2.f * farPlane * tanfov;
vertical *= 2.f * farPlane * tanfov;
btScalar aspect;
float width = float(renderer->getScreenWidth());
float height = float (renderer->getScreenHeight());
aspect = width / height;
hor*=aspect;
btVector3 rayToCenter = rayFrom + rayForward;
btVector3 dHor = hor * 1.f/width;
btVector3 dVert = vertical * 1.f/height;
btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
rayTo += btScalar(x) * dHor;
rayTo -= btScalar(y) * dVert;
return rayTo;
}
btVector3 Tire::getForce(
btScalar normal_load,
btScalar friction_coeff,
btScalar camber,
btScalar rot_velocity,
btScalar lon_velocity,
btScalar lat_velocity)
{
if (normal_load * friction_coeff < btScalar(1E-6))
{
slip = slip_angle = 0;
ideal_slip = ideal_slip_angle = 1;
fx = fy = fz = mz = 0;
vx = vy = 0;
return btVector3(0, 0, 0);
}
// limit input
normal_load = Clamp(normal_load, btScalar(0), btScalar(max_load));
camber = Clamp(camber, -max_camber, max_camber);
// sigma and alpha
btScalar denom = Max(btFabs(lon_velocity), btScalar(1E-3));
btScalar lon_slip_velocity = lon_velocity - rot_velocity;
btScalar sigma = -lon_slip_velocity / denom;
btScalar alpha = btAtan(lat_velocity / denom);
// force parameters
btScalar Fz = normal_load;
btScalar Fz0 = nominal_load;
btScalar dFz = (Fz - Fz0) / Fz0;
// pure slip
btScalar Dy, BCy, Shf;
btScalar Fx0 = PacejkaFx(sigma, Fz, dFz, friction_coeff);
btScalar Fy0 = PacejkaFy(alpha, camber, Fz, dFz, friction_coeff, Dy, BCy, Shf);
btScalar Mz0 = PacejkaMz(alpha, camber, Fz, dFz, friction_coeff, Fy0, BCy, Shf);
// combined slip
btScalar Gx = PacejkaGx(sigma, alpha);
btScalar Gy = PacejkaGy(sigma, alpha);
btScalar Svy = PacejkaSvy(sigma, alpha, camber, dFz, Dy);
btScalar Fx = Gx * Fx0;
btScalar Fy = Gy * Fy0 + Svy;
// ideal slip and angle
btScalar sigma_hat(0), alpha_hat(0);
getSigmaHatAlphaHat(normal_load, sigma_hat, alpha_hat);
slip = sigma;
slip_angle = alpha;
ideal_slip = sigma_hat;
ideal_slip_angle = alpha_hat;
fx = Fx;
fy = Fy;
fz = Fz;
mz = Mz0;
vx = lon_slip_velocity;
vy = lat_velocity;
return btVector3(Fx, Fy, Mz0);
}
