//----------------------------------------------------------------------------------------------------------------------
/// @brief If particles are inside each other moves them apart
/// @param _testParticle the particle we want to test
/// @param _particles our array of particles we wish to test against
public void initParticleCollision(particle _testParticle, List<particle> _particles)
{
Vector3 newPosP, newPosQ, currentPosQ,currentPosP,N;
float radius = _testParticle.getRadius();
for (int i=0; i<_particles.Count;i++)
{
N = _testParticle.getPos() - _particles[i].getPos();
if (N.sqrMagnitude > 0 && N.sqrMagnitude < radius)
{
currentPosP = _testParticle.getPos();
currentPosQ = _particles[i].getPos();
newPosP = (currentPosP + (N * (0.5f)));
newPosQ = (currentPosQ - (N * (0.5f)));
_testParticle.setPos(newPosP);
_particles[i].setPos(newPosQ);
}
}
}
/// @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<<"]");
}
//----------------------------------------------------------------------------------------------------------------------
/// @brief calculates the smoothing kernal for density, used in our SPH equations
/// @param _currentParticle the particle we wish to test for
/// @param _neighbour the particle we wish to test our _current particle against
/// @return the weighted viscosity vector the particle has on our _currentParticle
protected Vector3 calcViscosityKern(particle _currentParticle, particle _neighbour)
{
Vector3 r = _currentParticle.getPos() - _neighbour.getPos();
if(r.length() > m_smoothingLength)
{
return Vector3.zero;
}
float viscosityKern = -(945/(32*Mathf.PI * Mathf.Pow(m_smoothingLength,9))) * Mathf.Pow(((m_smoothingLength*m_smoothingLength) - (r.length()*r.length())),3) * ((3*(m_smoothingLength*m_smoothingLength)) - 7*(m_smoothingLength*m_smoothingLength));
return r * viscosityKern;
}
//----------------------------------------------------------------------------------------------------------------------
/// @brief calculates the smoothing kernal for density, used in our SPH equations
/// @param _currentParticle the particle we wish to test for
/// @param _neighbour the particle we wish to test our _current particle against
/// @return the weighted pressued gradient the particle has on our _currentParticle
protected Vector3 calcPressureKern(particle _currentParticle,particle _neighbour)
{
Vector3 r = _currentParticle.getPos() - _neighbour.getPos();
if (r.length()>m_smoothingLength)
{
return Vector3.zero;
}
float pressureKern = -(945/(32*Mathf.PI * Mathf.Pow(m_smoothingLength,9))) * Mathf.Pow(((m_smoothingLength*m_smoothingLength) - (r.length()*r.length())),3);
return r *(pressureKern);
}
//----------------------------------------------------------------------------------------------------------------------
/// @brief calculates the smoothing kernal for density, used in our SPH equations
/// @param _currentParticle the particle we wish to test for
/// @param _neighbour the particle we wish to test our _current particle against
/// @return the weight of influence the particle has on our _currentParticle
protected float calcDensityKern(particle _currentParticle, particle _neighbour)
{
Vector3 r = _currentParticle.getPos() - _neighbour.getPos();
if(r.length() > m_smoothingLength)
{
return 0;
}
float densityKern = (315 / (64 * Mathf.PI * Mathf.Pow(m_smoothingLength, 9))) * Mathf.Pow(((m_smoothingLength * m_smoothingLength) - (r.length() * r.length())), 3);
return densityKern;
}
