Bond::Bond(int d0, int d1, DEMsystem &dem_){
cerr << "!!!" << endl;
dem = &dem_;
status = 1;
initial_bond = dem->initialprocess;
bond_number = dem->n_bond;
dem->n_bond ++ ;
d[0] = d0, d[1] = d1;
dem->ct->on_connect( d[0], d[1] );
p_particle0 = dem->particle[d[0]];
p_particle1 = dem->particle[d[1]];
(*p_particle1).setBond(bond_number, d[0]);
(*p_particle0).setBond(bond_number, d[1]);
pp[0] = (*p_particle0).p_pos(); // pointer to postion of particle 0.
pu[0] = (*p_particle0).pu_back(); // pointer to u vector of particle 0.
pp[1] = (*p_particle1).p_pos();
pu[1] = (*p_particle1).pu_back();
r_vec = (*pp[1]) - (*pp[0]);
p_tor[0] = (*p_particle0).p_tor_angle_back();
p_tor[1] = (*p_particle1).p_tor_angle_back();
(*p_tor[0]) = 0;
(*p_tor[1]) = 0;
r = r_vec.norm();
e_normal = r_vec / r;
(*pu[0]) = e_normal;
(*pu[1]) = - e_normal;
u_vector[0] = (*p_particle0).orientation.ori_backward(*pu[0]);
u_vector[1] = (*p_particle1).orientation.ori_backward(*pu[1]);
if (initial_bond){
u_vector_initial[0] = u_vector[0];
u_vector_initial[1] = u_vector[1];
}
if (initial_bond)
para = dem->bond0;
else
para = dem->bond1;
sq_fsc = sq(para.fsc);
sq_mbc = sq(para.mbc);
sq_mtc = sq(para.mtc);
calcForce();
}
void Bond::regeneration(){
status = 2;
r_vec = (*pp[1]) - (*pp[0]);
r = r_vec.norm();
e_normal = r_vec/r;
(*pu[0]) = e_normal;
(*pu[1]) = - e_normal;
(*p_tor[0]) = 0.0;
(*p_tor[1]) = 0.0;
u_vector[0] = (*p_particle0).orientation.ori_backward(*pu[0]);
u_vector[1] = (*p_particle1).orientation.ori_backward(*pu[1]);
calcForce();
//regeneration_tuzuku = true;
}
void PotentialPlanner2::calcTotalForce()
{
Vector2D temp_force;
setVector2D(temp_force,0.0,0.0);
for (int i=0;i<pedestrians->size();++i)
{
temp_force = addVector2D(temp_force, calcForce( *((*pedestrians)[i]) ));
}
//ADD THE GOAL EFFECT
temp_force.y += 70.0*m_charge;
//ADD THE ROAD EFFECT
dd isInside = 1.0;
dd dx_left = car->getX()-PAVEMENT_LEFT_X_MAX-car->getWidth()/2;
dd dx_right = car->getX()-PAVEMENT_RIGHT_X_MIN+car->getWidth()/2;
if (!(car->getX()-car->getWidth()/2 > PAVEMENT_LEFT_X_MAX && car->getX()+car->getWidth()/2 < PAVEMENT_RIGHT_X_MIN)) isInside=-1.0;
temp_force.x += (10.0*10.0)*isInside*m_charge*abs(car->getV()*cos(car->getTheta()))/(dx_left*dx_left*dx_left);
temp_force.x += (10.0*10.0)*isInside*m_charge*abs(car->getV()*cos(car->getTheta()))/(dx_right*dx_right*dx_right);
//We use temp_force first to let the GUI only drawing the resultant force
m_force = temp_force;
}
// Physics thread that controls the motion of all the spheres.
void physicsThread(int threadNum)
{
int localFrame = 0;
// Define the normals for each of the walls.
static const Vector3 bottom(0, 1, 0);
static const Vector3 left(1, 0, 0);
static const Vector3 right(-1, 0, 0);
static const Vector3 front(0, 0, -1);
static const Vector3 back(0, 0, 1);
// Initialize the starting values for the wall and sphere, spring and damping values.
float wallSpringConst = UPPER_WALL_SPRING_CONST;
float sphereSpringConst = UPPER_SPHERE_SPRING_CONST;
float wallDampFactor = UPPER_WALL_DAMP_FACTOR;
float sphereDampFactor = UPPER_SPHERE_DAMP_FACTOR;
// The physics thread itself deals with reseting its frame counter.
// It will do this once a second.
float secondCheck = 0.0f;
#if _USE_MT
// If the physics function is being called as a thread, then we don't want it
// to finish after one pass.
int threadRunning = 0;
while(programRunning)
{
// If the simulation needs to be paused but the thread is running,
// stop.
if(pauseSimulation && threadRunning)
{
threadRunning = 0;
threadStopped();
}
// If the simulation needs to be running but the thread is stopped,
// start.
else if(!pauseSimulation && !threadRunning)
{
threadRunning = 1;
threadStarted();
}
// If the thread isn't running at this time we can't go any further and
// we'll just wait and check if the thread has started up next time the
// thread is executing.
if(!threadRunning)
{
boost::this_thread::yield();
continue;
}
#endif
localFrame++;
LARGE_INTEGER time;
QueryPerformanceCounter(&time);
// Get the time since this thread last executed and convert into
// seconds (from milliseconds).
float dt = (float)(((double)time.QuadPart - (double)updateTimes[threadNum].QuadPart) / freq);
dt *= 0.001;
// Adjust the spring and dampening values based on dt.
// This helps when going to lower frame rates and the overlap between
// the walls and spheres when using high frame rate values will cause
// the spheres to gain energy.
wallSpringConst = WALL_SPRING_GRAD * dt + WALL_SPRING_CONST;
sphereSpringConst = SPHERE_SPRING_GRAD * dt + SPHERE_SPRING_CONST;
wallDampFactor = WALL_DAMP_GRAD * dt + WALL_DAMP_CONST;
sphereDampFactor = SPHERE_DAMP_GRAD * dt + SPHERE_DAMP_CONST;
// As the gradients for the spring and dampening factors are negative,
// we want to clamp the lower bounds of the values. Otherwise at low
// framerates the values will be negative.
if (wallSpringConst < LOWER_WALL_SPRING_CONST)
wallSpringConst = LOWER_WALL_SPRING_CONST;
if (sphereSpringConst < LOWER_SPHERE_SPRING_CONST)
sphereSpringConst = LOWER_SPHERE_SPRING_CONST;
if (wallDampFactor < LOWER_WALL_DAMP_FACTOR)
wallDampFactor = LOWER_WALL_DAMP_FACTOR;
if (sphereDampFactor < LOWER_SPHERE_DAMP_FACTOR)
sphereDampFactor = LOWER_SPHERE_DAMP_FACTOR;
// If this is the 1st thread, then we will deal with the user controlled
// sphere here.
int startSphere = threadNum;
if(threadNum == 0)
{
// Skip calcuating the user sphere like a typical sphere.
startSphere += numPhysicsThreads;
// As the user controlled sphere only has a position and velocity
// that is either zero or constant, we can easily calculate its new
// position.
if(numSpheres > 0)
{
spherePositions[0] += sphereData[0].m_velocity * dt;
}
}
for(int i = startSphere; i < numSpheres; i += numPhysicsThreads)
{
// Calculate the radius of the sphere based on array index.
float radius = (float)((i % 3) + 1) * 0.5f;
float roomSize = ROOM_SIZE - radius;
Vector3 forces(0);
Vector3 &spherePosition = spherePositions[i];
SphereData &sphere = sphereData[i];
// Calculate the interim velocity.
Vector3 halfVelo = sphere.m_velocity + (0.5f * sphere.m_acc * dt);
spherePosition += halfVelo * dt;
Vector3 tempVelocity = sphere.m_velocity + (sphere.m_acc * dt);
float overlap;
// As the % operator is fairly slow we can take advantage here
// that we always know what the next value in the array is going
// to be and manually determine the mod.
int indexCounter = 0;
for(int j = 0; j < numSpheres; j++)
{
// And since we don't want the actual mod value but mod + 1
// we don't worry about resetting to 0.
indexCounter++;
if(indexCounter > 3)
indexCounter = 1;
// We don't want a sphere to check if it's collided with itself.
if(i == j)
continue;
SphereData &otherSphere = sphereData[j];
Vector3 toCentre = spherePosition - spherePositions[j];
// An unfortunately slow way of checking if the radius of the
// other sphere should be the other spheres actual radius or
// the user controlled spheres radius.
float otherRadius;
if(j == 0)
{
otherRadius = userSphereSize;
}
else
{
otherRadius = (float)(indexCounter) * 0.5f;
}
float combinedRadius = radius + otherRadius;
float lengthSqrd = lengthSquared(toCentre);
float combinedRadiusSqrd = combinedRadius * combinedRadius;
// A check to see if the spheres have overlapped at all.
float overlapSqrd = lengthSqrd - combinedRadiusSqrd;
if(overlapSqrd < 0)
{
#if _SSE
overlap = sqrt(lengthSqrd) - combinedRadius;
// We want to let the vector normalize itself in SSE
// because we can take advantage of the rsqrt instruction
// which will be quicker than loading in a float value
// and multiplying by the reciprocal.
Vector3 tangent = normalize(toCentre);
#else
// Here we can take advantage that we've already calculated
// the actual length value so that we have the overlap and
// can use that value again to normalize the tangent.
float len = sqrt(lengthSqrd);
overlap = len - combinedRadius;
Vector3 tangent = toCentre / len;
#endif
calcForce(forces, sphereSpringConst, sphereDampFactor, overlap, tangent, tempVelocity);
}
}
// Calculate the overlap with the walls using the normal of the wall
// and the walls distance from the origin. Should work best for SSE
// as it should not require any checking of individual elements of
// the vector.
overlap = dot3(spherePosition, bottom) + roomSize;
if(overlap < 0)
{
calcForce(forces, wallSpringConst, wallDampFactor, overlap, bottom, tempVelocity);
}
overlap = dot3(spherePosition, left) + roomSize;
if(overlap < 0)
{
calcForce(forces, wallSpringConst, wallDampFactor, overlap, left, tempVelocity);
}
overlap = dot3(spherePosition, right) + roomSize;
if(overlap < 0)
{
calcForce(forces, wallSpringConst, wallDampFactor, overlap, right, tempVelocity);
}
overlap = dot3(spherePosition, front) + roomSize;
if(overlap < 0)
{
calcForce(forces, wallSpringConst, wallDampFactor, overlap, front, tempVelocity);
}
overlap = dot3(spherePosition, back) + roomSize;
if(overlap < 0)
{
calcForce(forces, wallSpringConst, wallDampFactor, overlap, back, tempVelocity);
}
// Apply the accumulated forces to the acceleration.
sphere.m_acc = forces / (radius * 2.0f);
sphere.m_acc += GRAVITY;
// Calculate the final velocity value from the interim velocity
// and final acceleration.
sphere.m_velocity = halfVelo + (0.5f * sphere.m_acc * dt);
}
// Determin if we need to reset out physics frame counter.
secondCheck += dt;
if(secondCheck > 1.0f)
{
physicsFrames[threadNum] = 1;
secondCheck -= 1.0f;
}
else
{
physicsFrames[threadNum]++;
}
updateTimes[threadNum] = time;
// If we're running the physics function as a thread function then we
// need to keep it running, so this is the end of the while(programRunning) loop.
#if _USE_MT
}
#endif
}
void Bond::addContactForce(){
calcForce();
(*p_particle0).stackForce( force0, torque0);
(*p_particle1).stackForce(-force0, torque1);
}
