/// @brief calculates and updates a particles position and velocity depending on if it has a collision with the ground
/// @param _testParticle the particle we wish to update
/// @param _timeStep the time step between our calcuations, this is used in calculting the friction
protected void calculateWallCollision(particle _testParticle, float _timeStep, Vector4 _ground)
{
Vector3 currentPos = _testParticle.getPos();
Vector3 currentVel = _testParticle.getVel();
float radius = _testParticle.getRadius();
Vector3 Vel = _testParticle.getVel(), Pos = _testParticle.getPos();
Vector3 normal = new Vector3(_ground.x,_ground.y,_ground.z);
normal.Normalize();
//Test with ground
if ((currentPos.dot(normal) -_ground.w) < radius)
{
//-----------------Calculate Velocity------------------
//Calculate our new velocity with momentum included
Vel = -((currentVel.dot(normal))) * normal + (currentVel - (currentVel.cross(normal).cross(normal)));
Vel+= m_coefOfRest * currentVel.dot(normal) * normal;
//If moving parallel to our plane decrease speed due to friction unless it has already stopped
if(currentVel.sqrMagnitude > 0 && currentVel.dot(normal) == 0)
{
Vector3 VelNormalized = Vel;
VelNormalized.Normalize();
VelNormalized *=(-1);
Vector3 friction = new Vector3((1 - normal.x) * VelNormalized.x,(1-normal.y) * VelNormalized.y,(1-normal.z) * VelNormalized.z);
friction *=((m_coefOfFric*_timeStep)/(_testParticle.getMass()));
if(friction.length()<Vel.length())
{
Vel +=(friction);
}
else if(friction.length()>=Vel.length())
{
Vel.set(0,0,0);
}
}
_testParticle.setVel(Vel);
//---------------Calculate Position----------------------
//If particle has a velocity which is not parallel to our plane find its new position
if(currentVel.length() != 0 || currentVel.dot(normal) != 0)
{
Vector3 curPosRadius = currentPos - normal * (radius);
float t = (_ground.w - normal.dot(curPosRadius)) / normal.dot(normal);
Vector3 closestPoint = curPosRadius + normal *(t);
if(t > 0 && t < 1)
{
Pos = closestPoint + normal *(radius);
}
else
{
Pos = closestPoint + normal *(radius);
}
_testParticle.setPos(Pos);
}
}
}
