static void ApplyGravity(const NewtonBody* const body, dFloat timestep, int threadIndex)
{
// apply gravity force to the body
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
dVector gravityForce(0.0f, -9.8f * mass, 0.0f, 0.0f);
NewtonBodySetForce(body, &gravityForce[0]);
}
// the end application need to overload this function from dNetwonDynamicBody
void OnForceAndTorque (dFloat timestep, int threadIndex)
{
// apply gravity force to the body
dFloat mass;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
GetMassAndInertia (mass, Ixx, Iyy, Izz);
dVector gravityForce (0.0f, -9.8f * mass, 0.0f, 0.0f);
SetForce (&gravityForce[0]);
}
// callback to apply external forces to body
void ApplyForceAndTorqueCallback (const NewtonBody* body, dFloat timestep, int threadIndex)
{
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
// for this tutorial the only external force in the Gravity
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
dVector gravityForce (0.0f, mass * GRAVITY, 0.0f, 1.0f);
NewtonBodySetForce(body, &gravityForce[0]);
}
void Body::__applyForceAndTorqueCallback(const NewtonBody* body, dFloat timestep, int threadIndex)
{
//std::cout << "\tforce " << threadIndex << " " << body << std::endl;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);
Vec4f gravityForce(0.0f, mass * newton::gravity, 0.0f, 1.0f);
NewtonBodySetForce(body, &gravityForce[0]);
//std::cout << "\tforce end " << threadIndex << " " << body << std::endl;
}
///---------------------------------------------------------------------------------
///
///---------------------------------------------------------------------------------
StateVector3D SmokeUpdateStrategy::CalculateDerivative( const StateVector3D& initialState )
{
Vector3 pos = initialState.s1;
Vector3 vel = initialState.s2;
Vector3 gravityForce( 0.0f, -9.81f, 0.0f );
if (!m_gravityEnabled)
gravityForce = Vector3::ZERO;
Vector3 wind = -m_windC * (vel - m_windForce);
Vector3 force = gravityForce + wind;
StateVector3D derivative( vel, force );
return derivative;
}
// callback to apply external forces to body
void ApplyForceAndTorqueCallback (const NewtonBody* body, float timestep, int threadIndex)
{
float Ixx;
float Iyy;
float Izz;
float mass;
// for this tutorial the only external force in the Gravity
NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
float y=0.0;
float x=0.0;
if(glfwGetKey(GLFW_KEY_UP))
y=1.0;
if(glfwGetKey(GLFW_KEY_DOWN))
y=-1.0;
if(glfwGetKey(GLFW_KEY_LEFT))
x=-1.0;
if(glfwGetKey(GLFW_KEY_RIGHT))
x=1.0;
glm::vec4 gravityForce (300.0f*x*mass, 300.0f*y*mass, mass * -100.0f, 1.0f);
NewtonBodySetForce(body, &gravityForce[0]);
}
void ParticleSimulation::computeForces()
{
for (int i = 0; i < ps.num_gravity; i++)
{
//ps.gravitys[i].applyForce(ps.gravitys[i].p);
gravityForce(ps.gravitys[i].p);
}
for (int i = 0; i < ps.num_friction; i++)
{
//ps.frictions[i].applyForce(ps.frictions[i].p);
frictionForce(ps.frictions[i].p);
}
//for (int i = 0; i < ps.num_hookSpring; i++)
//{
// //ps.hookSprings[i].applyForce(ps.hookSprings[i].p1, ps.hookSprings[i].p2, ps.hookSprings[i].kd, ps.hookSprings[i].ks, ps.hookSprings[i].r);
// hookSpringForce(ps.hookSprings[i].p1, ps.hookSprings[i].p2, ps.hookSprings[i].kd, ps.hookSprings[i].ks, ps.hookSprings[i].r);
//}
//if (haengt)
//{
// for (int i = 0; i < num_points; i++)
// {
// ps.particles[i].force_acc[0] = 0;
// ps.particles[i].force_acc[1] = 0;
// ps.particles[i].force_acc[2] = 0;
// }
//}
//if (kette)
//{
// ps.particles[0].force_acc[0] = 0;
// ps.particles[0].force_acc[1] = 0;
// ps.particles[0].force_acc[2] = 0;
// ps.particles[num_points - 1].force_acc[0] = 0;
// ps.particles[num_points - 1].force_acc[1] = 0;
// ps.particles[num_points - 1].force_acc[2] = 0;
//}
}
void Scene::render(){
for(int t = 0; t < timeStep; t++){ //for every timestep
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // clear the color buffer
glMatrixMode(GL_MODELVIEW); // indicate we are specifying camera transformations
glLoadIdentity();
gluLookAt(0.0, 1.0, -0.25, 0.0, 0.0, -1.0, 0.0, 0.0, -1.0 );
// if(t % 5 == 0) {
// init();
// }
//Draw the boundaries
drawBoundaries();
vector<vector<Particle * > > neighbors;
//density calculations
#pragma omp for
for(int i = 0; i < particles->size(); i++){ //for every particle
Particle *particle = particles->at(i);
double density = MASS;
vector<Particle *> findNeighs;
for(int j = 0; j < particles->size(); j++){ //comparison to all other particles to see if they're close enough to effect the density
Particle *tempParticle = particles->at(j);
double dist = particle->getDistance(*tempParticle);
if (dist <= H && i != j){ //if the particle is close enough, add its mass * kernel to the density
double kern = particle->getKernel(dist);
density += tempParticle->getMass() * kern;
findNeighs.push_back(tempParticle);
}
}
neighbors.push_back(findNeighs);
particle->setDensity(density);
}
//second iteration of particles and only their neighbors
#pragma omp for
for(int i = 0; i < particles->size(); i++){
Particle *particle = particles->at(i);
Vector3f position = particle->getPosition();
Vector3f velocity = particle->getVelocity();
//http://stackoverflow.com/questions/17565664/gluproject-and-2d-display
//Render particle
GLdouble posX, posY, posZ;//3D point
posX=convert(position.x(), WIDTH);
posY=convert(position.y(), HEIGHT);
posZ=convert(position.z(), LENGTH);
glPushMatrix();
glTranslated(posX, posY, posZ);
glutSolidSphere(SRADIUS, 10, 10);
glPopMatrix();
//Force calculations
Vector3f viscosityForce = Vector3f::Zero();
Vector3f pressureForce = Vector3f::Zero();
Vector3f surfaceNormal = Vector3f::Zero();
double colorField = 0;
double pressureJ = particle->calcPressure();
vector<Particle * > curNeighs = neighbors[i];
for(int j = 0; j < curNeighs.size(); j++){//currNeighs.size(); j++){
Particle *tempParticle = curNeighs[j];// currNeighs[j]->p;
double tempMass = tempParticle->getMass();
double tempDens = tempParticle->getDensity();
Vector3f tempVel = tempParticle->getVelocity();
double dist = particle->getDistance(*tempParticle);
double kern = particle->getKernel(dist);
colorField += tempMass / tempDens * kern;
//Pressure and surfaceNormal
Vector3f rij = tempParticle->getPosition() - position;
Vector3f kernDerive = particle->getKernDerive(dist, rij);
double pressureK = tempParticle->calcPressure();
pressureForce += tempMass * (pressureJ + pressureK) / (2 * tempDens) * kernDerive;
surfaceNormal += tempMass / tempDens * kernDerive;
//Viscosity
double kernSecond = particle->getKernSecond(dist);
viscosityForce += (tempVel - velocity) * tempMass / tempDens * kernSecond;
}
pressureForce *= -1;
viscosityForce *= VISC;
Vector3f surfaceTension = Vector3f::Zero();
Vector3f gravityForce(0, particle->getDensity() * GRAVITY, 0);
// cout << "pForce: " << pressureForce << endl;
// //cout << "dens: " << particle->getDensity() << endl;
// cout << "glForce: " << gravityForce << endl;
// cout << "vForce: " << viscosityForce << endl;
//Update next position
Vector3f totalForce = gravityForce + pressureForce + viscosityForce;
//cout << "totalForce: " << totalForce << endl;
Vector3f acceleration = totalForce/particle->getDensity();
//cout << "1. " << particle->getVelocity() << endl;
velocity = velocity + DELTAT * acceleration; //maybe implement some kind of terminal velocity?
//cout << "2. " << velocity << endl;
Vector3f newPosition = position + DELTAT * velocity;
//Boundary check next position
bool bounce = false;
while((newPosition.x() - RADIUS <= LEFT) || (newPosition.y() - RADIUS <= BOTTOM) || (newPosition.x() + RADIUS >= RIGHT) || (newPosition.y() + RADIUS >= TOP) || (newPosition.z() + RADIUS <= BACK) || (newPosition.z() - RADIUS >= FRONT)){
bounce = true;
velocity *= 0.9;
newPosition = position + DELTAT * velocity;
}
if(bounce) velocity *= -1;
// if(newPosition.y() - RADIUS <= BOTTOM){
// int boundTime = (BOTTOM - position.y()) / velocity;
// Vector3f collision = position + boundTime * velocity;
// Vector3f collNorm(collision.x(), 1, collision.z()).normalized();
// double penDist = newPosition.dist(collision);
// newPosition = newPosition + penDist * collNorm;
// velocity = velocity - 0.3 * (velocity.dot(collNorm)) * collNorm;
// }
particle->setPosition(newPosition);
particle->setVelocity(velocity);
}
saveImage(t);
glFlush();
glutSwapBuffers();
glPopMatrix();
}
}
