/// @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);
}
}
}
//----------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------
/// Calculates the forces for a particle using leap frog method of integration
/// _currentParticle - The particle we are going to calculate the forces for
/// _particleIndex - Vector of all our particles if we want to brut force or the neighbour particles for SPH
/// _timeStep - the time step between updates
public void calcForces(particle _currentParticle, List<particle> _particleIndex, float _timeStep)
{
float densityCurrent = 0;
var gravitiy = new Vector3 (0f,-9.8f,0f);
var pressureGrad = Vector3.zero;
var viscosity = Vector3.zero;
var viscosityVector = Vector3.zero;
var Acceleration = Vector3.zero;
for(int i = 0; i<_particleIndex.Count;i++){
// Calculate desity of all particles
if(_currentParticle!=_particleIndex[i]){
densityCurrent += _particleIndex[i].getMass()*calcDensityKern(_currentParticle, _particleIndex[i]);
}
}
_currentParticle.setDensity(densityCurrent);
var pressTemp = Vector3.zero;
var visTemp = Vector3.zero;
float pressureCurrent, pressureNeighbour, pressure;
for(int i=0; i<_particleIndex.Count;i++)
{
// Calcualate pressure
pressureCurrent = m_gasConstant * (_currentParticle.getDensity() - _currentParticle.getRestDensity());
pressureNeighbour = m_gasConstant * (_particleIndex[i].getDensity() - _particleIndex[i].getRestDensity());
pressure = ((pressureCurrent /(pressureCurrent*pressureCurrent)) + (pressureNeighbour /(pressureNeighbour*pressureNeighbour)))*(_particleIndex[i].getMass());
pressTemp = calcPressureKern(_currentParticle,_particleIndex[i]);
//pressTemp*= pressure; //!omfg not an assignment
pressureGrad +=(pressTemp);
// Calculate viscosiy vector
viscosityVector = _particleIndex[i].getVel() - _currentParticle.getVel();
viscosityVector *=(_particleIndex[i].getMass());
viscosityVector /=(_particleIndex[i].getDensity());
// Calculate viscosiy
visTemp = calcViscosityKern(_currentParticle,_particleIndex[i]);
//visTemp = visTemp.cross(viscosityVector); //!omfg not an assignment
viscosity +=(visTemp);
}
viscosity *=m_viscosityCoefficient;
//Calcualate external force if the is one
var Forces = Vector3.zero;
if(m_externalForceStrenth != 0 && m_externalForceRadius != 0)
{
Forces = _currentParticle.getPos() - m_externalForcePos;
if (Forces.length()<=m_externalForceRadius && Forces.length() > 0)
{
//find the direction the force is in
Forces.Normalize();
//Forces *=((m_externalForceRadius-Forces.length())/m_externalForceRadius);
Forces *=(m_externalForceStrenth);
if(m_externalForcePush == false)
{
Forces =-Forces;
}
}
else
{
Forces.set(0f,0f,0f);
}
}
// Calculate our acceleration
Acceleration = gravitiy - pressureGrad + viscosity + Forces; //tweak center
//---------------leap frog integration------------------------
//Calculate velocity
var VelHalfBack = _currentParticle.getVel() - _currentParticle.getAcc() * _timeStep/2;
var VelHalfForward = VelHalfBack + Acceleration * _timeStep;
var Velocity = (VelHalfBack + VelHalfForward) * 0.5f;
//Calculate our new position
var Position = _currentParticle.getPos() + VelHalfForward *(_timeStep);
_currentParticle.setVel(Velocity);
_currentParticle.setPos(Position);
//---------------Debuging----------------
// Debug.Log("the viscosity is "+"["<<viscosity.m_x+","<<viscosity.m_y+","<<viscosity.m_z+"]");
// Debug.Log("the pressure grad is "+"["<<pressureGrad.m_x<<","<<pressureGrad.m_y<<","<<pressureGrad.m_z<<"]");
// Debug.Log("the accelleration is "+"["<<Acceleration.m_x<<","<<Acceleration.m_y<<","<<Acceleration.m_z<<"]");
// Debug.Log("the velocity is "+"["<<Velocity.m_x+","<<Velocity.m_y<<","<<Velocity.m_z<<"]");
}