void Ball::Collide(CollisionEvent *coll)
{
Ball *pball = coll->ball;
const Vertex3Ds vnormal = coll->normal[0];
if (pball->fFrozen)
return;
// correct displacements, mostly from low velocity, alternative to true acceleration processing
// target ball to object ball delta velocity
const Vertex3Ds impulse = pball->collisionMass * pball->vel - collisionMass * vel;
float dot = impulse.Dot(vnormal);
if (dot >= -C_LOWNORMVEL ) // nearly receding ... make sure of conditions
{ // otherwise if clearly approaching .. process the collision
if (dot > C_LOWNORMVEL) return; //is this velocity clearly receding (i.e must > a minimum)
#ifdef C_EMBEDDED
if (coll->distance < -C_EMBEDDED)
dot = -C_EMBEDSHOT; // has ball become embedded???, give it a kick
else return;
#endif
}
#ifdef C_DISP_GAIN
float edist = -C_DISP_GAIN * coll->distance;
if (edist > 1.0e-4f)
{
if (edist > C_DISP_LIMIT)
edist = C_DISP_LIMIT; // crossing ramps, delta noise
if (!fFrozen) edist *= 0.5f; // if the hitten ball is not frozen
pball->pos += edist * vnormal;// push along norm, back to free area
// use the norm, but is not correct, but cheaply handled
}
edist = -C_DISP_GAIN * m_coll.distance; // noisy value .... needs investigation
if (!fFrozen && edist > 1.0e-4f)
{
if (edist > C_DISP_LIMIT)
edist = C_DISP_LIMIT; // crossing ramps, delta noise
edist *= 0.5f;
pos -= edist * vnormal; // pull along norm, back to free area
}
#endif
const float averageMass = (collisionMass + pball->collisionMass)*0.5f;
const float impulse1 = ((float)(-1.8 * 0.5) * dot) * pball->collisionMass / (averageMass * collisionMass);
float impulse2 = ((float)(-1.8 * 0.5) * dot) * collisionMass / (averageMass * pball->collisionMass);
if (!fFrozen)
{
vel -= impulse1 * vnormal;
m_fDynamic = C_DYNAMIC;
}
else impulse2 += impulse1;
pball->vel += impulse2 * vnormal;
pball->m_fDynamic = C_DYNAMIC;
}
void Ball::AngularAcceleration(const Vertex3Ds& hitnormal)
{
const Vertex3Ds bccpd = -radius * hitnormal; // vector ball center to contact point displacement
const float bnv = vel.Dot(hitnormal); // ball normal velocity to hit face
const Vertex3Ds bvn = bnv * hitnormal; // project the normal velocity along normal
const Vertex3Ds bvt = vel - bvn; // calc the tangent velocity
Vertex3Ds bvT = bvt; // ball tangent velocity Unit Tangent
bvT.Normalize();
const Vertex3Ds bstv = // ball surface tangential velocity
CrossProduct(m_angularvelocity, bccpd); // velocity of ball surface at contact point
const float dot = bstv.Dot(bvT); // speed ball surface contact point tangential to contact surface point
const Vertex3Ds cpvt = dot * bvT; // contact point velocity tangential to hit face
const Vertex3Ds slideVel = bstv - cpvt; // contact point slide velocity with ball center velocity -- slide velocity
// If the point and the ball are travelling in opposite directions,
// and the point's velocity is at least the magnitude of the balls,
// then we have a natural roll
Vertex3Ds cpctrv = -slideVel; //contact point co-tangential reverse velocity
if (vel.LengthSquared() > (float)(0.7*0.7))
{
// Calculate the maximum amount the point velocity can change this
// time segment due to friction
Vertex3Ds FrictionForce = cpvt + bvt;
// If the point can change fast enough to go directly to a natural roll, then do it.
if (FrictionForce.LengthSquared() > (float)(ANGULARFORCE*ANGULARFORCE))
FrictionForce.Normalize(ANGULARFORCE);
cpctrv -= FrictionForce;
}
// Divide by the inertial tensor for a sphere in order to change
// linear force into angular momentum
cpctrv *= (float)(2.0/5.0); // Inertial tensor for a sphere
const Vertex3Ds vResult = CrossProduct(bccpd, cpctrv); // ball center contact point displacement X reverse contact point co-tan vel
m_angularmomentum *= 0.99f;
m_angularmomentum += vResult; // add delta
m_angularvelocity = m_inverseworldinertiatensor.MultiplyVector(m_angularmomentum);
}
float Ball::HitTest(const Ball * pball_, float dtime, CollisionEvent& coll)
{
Ball * pball = const_cast<Ball*>(pball_); // HACK; needed below
Vertex3Ds d = pos - pball->pos; // delta position
Vertex3Ds dv = vel - pball->vel; // delta velocity
float bcddsq = d.LengthSquared(); // square of ball center's delta distance
float bcdd = sqrtf(bcddsq); // length of delta
if (bcdd < 1.0e-8f) // two balls center-over-center embedded
{ //return -1;
d.z = -1.0f; // patch up
pball->pos.z -= d.z; // lift up
bcdd = 1.0f; // patch up
bcddsq = 1.0f; // patch up
dv.z = 0.1f; // small speed difference
pball->vel.z -= dv.z;
}
float b = dv.Dot(d); // inner product
const float bnv = b/bcdd; // normal speed of balls toward each other
if ( bnv > C_LOWNORMVEL) return -1.0f; // dot of delta velocity and delta displacement, postive if receding no collison
const float totalradius = pball->radius + radius;
const float bnd = bcdd - totalradius; // distance between ball surfaces
float hittime;
if (bnd < (float)PHYS_TOUCH) // in contact???
{
if (bnd <= (float)(-PHYS_SKIN*2.0))
return -1.0f; // embedded too deep
if ((fabsf(bnv) > C_CONTACTVEL) // >fast velocity, return zero time
//zero time for rigid fast bodies
|| (bnd <= (float)(-PHYS_TOUCH)))
hittime = 0; // slow moving but embedded
else
hittime = bnd/(float)(2.0*PHYS_TOUCH) + 0.5f; // don't compete for fast zero time events
}
else
{
// find collision time as solution of quadratic equation
// at^2 + bt + c = 0
const float a = dv.LengthSquared(); // square of differential velocity
if (a < 1.0e-8f) return -1.0f; // ball moving really slow, then wait for contact
const float c = bcddsq - totalradius*totalradius; //first contact test: square delta position - square of radii
b *= 2.0f; // two inner products
float result = b*b - 4.0f*a*c; // squareroot term (discriminant) in quadratic equation
if (result < 0.0f) return -1.0f; // no collision path exist
result = sqrtf(result);
// compute the two solutions to the quadratic equation
float time1 = (-b + result)/(2.0f * a);
const float time2 = (-b - result)/(2.0f * a);
// choose smallest non-negative solution
hittime = std::min(time1, time2);
if (hittime < 0)
hittime = std::max(time1, time2);
if (infNaN(hittime) || hittime < 0 || hittime > dtime)
return -1.0f; // .. was some time previous || beyond the next physics tick
}
const Vertex3Ds hitPos = pball->pos + hittime * dv; // new ball position
//calc unit normal of collision
coll.normal[0] = hitPos - pos;
coll.normal[0].Normalize();
coll.distance = bnd; //actual contact distance
coll.hitRigid = true; //rigid collision type
return hittime;
}
float HitTriangle::HitTest(const Ball * pball, float dtime, CollisionEvent& coll)
{
if (!m_fEnabled) return -1.0f;
const float bnv = normal.Dot(pball->m_vel); // speed in Normal-vector direction
if (bnv >= C_CONTACTVEL) // return if clearly ball is receding from object
return -1.0f;
// Point on the ball that will hit the polygon, if it hits at all
const float bRadius = pball->m_radius;
Vertex3Ds hitPos = pball->m_pos - bRadius * normal; // nearest point on ball ... projected radius along norm
const float bnd = normal.Dot( hitPos - m_rgv[0] ); // distance from plane to ball
float hittime;
if (bnd < -pball->m_radius/**2.0f*/) //!! *2 necessary?
return -1.0f; // (ball normal distance) excessive pentratration of object skin ... no collision HACK
bool isContact = false;
if (bnd <= (float)PHYS_TOUCH)
{
if (fabsf(bnv) <= C_CONTACTVEL)
{
hittime = 0;
isContact = true;
}
else if (bnd <= 0)
hittime = 0; // zero time for rigid fast bodies
else
hittime = bnd / -bnv;
}
else if (fabsf(bnv) > C_LOWNORMVEL ) // not velocity low?
hittime = bnd / -bnv; // rate ok for safe divide
else
return -1.0f; // wait for touching
if (infNaN(hittime) || hittime < 0 || hittime > dtime)
return -1.0f; // time is outside this frame ... no collision
hitPos += hittime * pball->m_vel; // advance hit point to contact
// check if hitPos is within the triangle
// Compute vectors
const Vertex3Ds v0 = m_rgv[2] - m_rgv[0];
const Vertex3Ds v1 = m_rgv[1] - m_rgv[0];
const Vertex3Ds v2 = hitPos - m_rgv[0];
// Compute dot products
const float dot00 = v0.Dot(v0);
const float dot01 = v0.Dot(v1);
const float dot02 = v0.Dot(v2);
const float dot11 = v1.Dot(v1);
const float dot12 = v1.Dot(v2);
// Compute barycentric coordinates
const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
const float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
const float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
// Check if point is in triangle
const bool pointInTri = (u >= 0.f) && (v >= 0.f) && (u + v <= 1.f);
if (pointInTri)
{
coll.hitnormal = normal;
coll.hitdistance = bnd; // 3dhit actual contact distance ...
//coll.hitRigid = true; // collision type
if (isContact)
{
coll.isContact = true;
coll.hitvelocity.z = bnv;
}
return hittime;
}
else
return -1.0f;
}
float HitPoint::HitTest(const Ball * pball, float dtime, CollisionEvent& coll)
{
if (!m_fEnabled)
return -1.0f;
const Vertex3Ds dist = pball->m_pos - m_p; // relative ball position
const float bcddsq = dist.LengthSquared(); // ball center to line distance squared
const float bcdd = sqrtf(bcddsq); // distance ball to line
if (bcdd <= 1.0e-6f)
return -1.0f; // no hit on exact center
const float b = dist.Dot(pball->m_vel);
const float bnv = b/bcdd; // ball normal velocity
if (bnv > C_CONTACTVEL)
return -1.0f; // clearly receding from radius
const float bnd = bcdd - pball->m_radius; // ball distance to line
const float a = pball->m_vel.LengthSquared();
float hittime = 0;
bool isContact = false;
if (bnd < (float)PHYS_TOUCH) // already in collision distance?
{
if (fabsf(bnv) <= C_CONTACTVEL)
{
isContact = true;
hittime = 0;
}
else
hittime = std::max(0.0f, -bnd / bnv); // estimate based on distance and speed along distance
}
else
{
if (a < 1.0e-8f)
return -1.0f; // no hit - ball not moving relative to object
float time1, time2;
if (!SolveQuadraticEq(a, 2.0f*b, bcddsq - pball->m_radius*pball->m_radius, time1, time2))
return -1.0f;
hittime = (time1*time2 < 0) ? max(time1,time2) : min(time1,time2); // find smallest nonnegative solution
}
if (infNaN(hittime) || hittime < 0 || hittime > dtime)
return -1.0f; // contact out of physics frame
const Vertex3Ds hitPos = pball->m_pos + hittime * pball->m_vel;
coll.hitnormal = hitPos - m_p;
coll.hitnormal.Normalize();
coll.isContact = isContact;
if (isContact)
coll.hitvelocity.z = bnv;
coll.hitdistance = bnd; // actual contact distance
//coll.hitRigid = true;
return hittime;
}
float HitCircle::HitTestBasicRadius(const Ball * pball, float dtime, CollisionEvent& coll,
bool direction, bool lateral, bool rigid)
{
if (!m_fEnabled || pball->m_frozen) return -1.0f;
Vertex3Ds c(center.x, center.y, 0.0f);
Vertex3Ds dist = pball->m_pos - c; // relative ball position
Vertex3Ds dv = pball->m_vel;
float targetRadius;
bool capsule3D;
if (!lateral && pball->m_pos.z > zhigh)
{
capsule3D = true; // handle ball over target?
//const float hcap = radius*(float)(1.0/5.0); // cap height to hit-circle radius ratio
//targetRadius = radius*radius/(hcap*2.0f) + hcap*0.5f; // c = (r^2+h^2)/(2*h)
targetRadius = radius*(float)(13.0/5.0); // optimized version of above code
//c.z = zhigh - (targetRadius - hcap); // b = c - h
c.z = zhigh - radius*(float)(12.0/5.0); // optimized version of above code
dist.z = pball->m_pos.z - c.z; // ball rolling point - capsule center height
}
else
{
capsule3D = false;
targetRadius = radius;
if (lateral)
targetRadius += pball->m_radius;
dist.z = dv.z = 0.0f;
}
const float bcddsq = dist.LengthSquared(); // ball center to circle center distance ... squared
const float bcdd = sqrtf(bcddsq); // distance center to center
if (bcdd <= 1.0e-6f)
return -1.0f; // no hit on exact center
const float b = dist.Dot(dv);
const float bnv = b/bcdd; // ball normal velocity
if (direction && bnv > C_LOWNORMVEL)
return -1.0f; // clearly receding from radius
const float bnd = bcdd - targetRadius; // ball normal distance to
const float a = dv.LengthSquared();
float hittime = 0;
bool fUnhit = false;
bool isContact = false;
// Kicker is special.. handle ball stalled on kicker, commonly hit while receding, knocking back into kicker pocket
if (m_ObjType == eKicker && bnd <= 0 && bnd >= -radius && a < C_CONTACTVEL*C_CONTACTVEL )
{
if (pball->m_vpVolObjs) pball->m_vpVolObjs->RemoveElement(m_pObj); // cause capture
}
if (rigid && bnd < (float)PHYS_TOUCH) // positive: contact possible in future ... Negative: objects in contact now
{
if (bnd < -pball->m_radius/**2.0f*/) //!! *2 necessary?
return -1.0f;
else if (fabsf(bnv) <= C_CONTACTVEL)
{
isContact = true;
hittime = 0;
}
else
hittime = std::max(0.0f, -bnd / bnv); // estimate based on distance and speed along distance
}
else if (m_ObjType >= eTrigger // triggers & kickers
&& pball->m_vpVolObjs && ((bnd < 0.f) == (pball->m_vpVolObjs->IndexOf(m_pObj) < 0)))
{ // here if ... ball inside and no hit set .... or ... ball outside and hit set
if (fabsf(bnd-radius) < 0.05f) // if ball appears in center of trigger, then assumed it was gen'ed there
{
if (pball->m_vpVolObjs)
pball->m_vpVolObjs->AddElement(m_pObj); //special case for trigger overlaying a kicker
} // this will add the ball to the trigger space without a Hit
else
{
hittime = 0;
fUnhit = (bnd > 0.f); // ball on outside is UnHit, otherwise it's a Hit
}
}
else
{
if((!rigid && bnd * bnv > 0.f) || // (outside and receding) or (inside and approaching)
(a < 1.0e-8f)) return -1.0f; // no hit ... ball not moving relative to object
float time1, time2;
if (!SolveQuadraticEq(a, 2.0f*b, bcddsq - targetRadius*targetRadius, time1, time2))
return -1.0f;
fUnhit = (time1*time2 < 0.f);
hittime = fUnhit ? max(time1,time2) : min(time1,time2); // ball is inside the circle
}
if (infNaN(hittime) || hittime < 0.f || hittime > dtime)
return -1.0f; // contact out of physics frame
const float hitz = pball->m_pos.z - pball->m_radius + pball->m_vel.z * hittime; // rolling point
if(((hitz + pball->m_radius *1.5f) < zlow) ||
(!capsule3D && (hitz + pball->m_radius*0.5f) > zhigh) ||
(capsule3D && (pball->m_pos.z + pball->m_vel.z * hittime) < zhigh)) return -1.0f;
const float hitx = pball->m_pos.x + pball->m_vel.x*hittime;
const float hity = pball->m_pos.y + pball->m_vel.y*hittime;
const float sqrlen = (hitx - c.x)*(hitx - c.x) + (hity - c.y)*(hity - c.y);
if (sqrlen > 1.0e-8f) // over center???
{ // no
const float inv_len = 1.0f/sqrtf(sqrlen);
coll.hitnormal.x = (hitx - c.x)*inv_len;
coll.hitnormal.y = (hity - c.y)*inv_len;
coll.hitnormal.z = 0.0f;
}
else
{ // yes, over center
coll.hitnormal.x = 0.0f; // make up a value, any direction is ok
coll.hitnormal.y = 1.0f;
coll.hitnormal.z = 0.0f;
}
if (!rigid) // non rigid body collision? return direction
coll.hitvelocity.x = fUnhit ? 1.0f : 0.0f; // UnHit signal is receding from target
coll.isContact = isContact;
if (isContact)
coll.hitvelocity.z = bnv;
coll.hitdistance = bnd; //actual contact distance ...
//coll.hitRigid = rigid; // collision type
return hittime;
}