//---------------------------------------------------------------------------------------------------------------------- /// @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); } } }