void RaycastCar::updateVehicle( btScalar step )
{
m_currentVehicleSpeedKmHour = btScalar(3.6f) * getRigidBody()->getLinearVelocity().length();
const btTransform & chassisTrans = getChassisWorldTransform();
btVector3 forwardW(chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]);
if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.0f))
m_currentVehicleSpeedKmHour *= btScalar(-1.0f);
for (int i = 0; i < getNumWheels(); i++)
{
updateWheelTransform(i, false);
btScalar depth;
depth = rayCast(m_wheelInfo[i]);
}
update_suspension(step);
update_engine(step);
update_forces(step);
apply_impulses(step);
}
void calculate_forces(){
EngineBrute calculations = EngineBrute(stars, stars_count, gravity_constant);
update_forces(calculations);
}
void compute_accel(sim_state_t* state, sim_param_t* params) {
// Unpack basic parameters
const float h = params->h;
const float rho0 = params->rho0;
// const float k = params->k;
// const float mu = params->mu;
const float g = params->g;
// const float mass = state->mass;
const float h2 = params->h2;
// Unpack system state
particle_t* p = state->part;
particle_t** hash = state->hash;
const int n = state->n;
// Rehash the particles
hash_particles(state, h);
// Compute density and color
compute_density(state, params);
// Constants for interaction term
const float C0 = params->C0;
const float Cp = params->Cp;
const float Cv = params->Cv;
// Start with gravity and surface forces
for (int i = 0; i < n; ++i)
{
vec3_set(p[i].a, 0, -g, 0);
}
// Accumulate forces
#ifdef USE_BUCKETING
/* BEGIN TASK */
// Start multi-threaded
// Iterate over each bucket, and within each bucket each particle
#pragma omp parallel
{
// Get the thread ID and total threads
int thread_id = omp_get_thread_num();
int total_threads = omp_get_num_threads();
// Get the appropriate forces vector
float* forces = forces_all + thread_id * (state->n * 3);
memset(forces, 0, sizeof(float) * state->n * 3);
// Get the dedupe buffer for this thread
char* usedBinID = used_bin_id_flags + (thread_id * HASH_SIZE);
// Create storage for neighbor set
unsigned buckets[MAX_NBR_BINS];
unsigned numbins;
// Process all buckets that the thread is responsible for
for (int iter_bucket = thread_id; iter_bucket < HASH_SIZE; iter_bucket += total_threads)
{
for (particle_t* pi = hash[iter_bucket]; pi != NULL ; pi = pi->next)
{
// Get the position of the pi force accumulator
unsigned diffPosI = pi - state->part;
float* pia = forces + 3 * diffPosI;
// Compute equal and opposite forces for the particle,
// first get neighbors
numbins = particle_neighborhood(buckets, pi, h, usedBinID);
for (int j = 0; j < numbins; ++j)
{
// Get the neighbor particles
unsigned bucketid = buckets[j];
for (particle_t* pj = hash[bucketid]; pj != NULL ; pj = pj->next) {
// Compute forces only if appropriate
if (pi < pj && abs(pi->ix - pj->ix) <= 1
&& abs(pi->iy - pj->iy) <= 1
&& abs(pi->iz - pj->iz) <= 1)
{
// Get the position of the pj force accumulator
unsigned diffPosJ = pj - state->part;
float* pja = forces + 3 * diffPosJ;
// Accumulate forces
update_forces(pi, pj, h2, rho0, C0, Cp, Cv, pia, pja);
}
}
}
}
}
// Accumulate values in vector
for (int iter_particle = 0; iter_particle < state->n; ++iter_particle)
{
particle_t* cur_particle = state->part + iter_particle;
float* pia = forces + 3 * iter_particle;
if(pia[0] != 0 || pia[1] != 0 || pia[2] != 0)
{
omp_set_lock(&cur_particle->lock);
vec3_saxpy(cur_particle->a, 1, pia);
omp_unset_lock(&cur_particle->lock);
}
}
}
/* END TASK */
#else
for (int i = 0; i < n; ++i) {
particle_t* pi = p+i;
for (int j = i+1; j < n; ++j) {
particle_t* pj = p+j;
update_forces(pi, pj, h2, rho0, C0, Cp, Cv, pi->a, pj->a);
}
}
#endif
}
int main()
{
int i;
dt = 1e-13;
H = 5e4;
t_max = 500.0*100.*dt;
Lx = Ly = Lz = pow(50.0/1e10,0.333333);
assert(N % 2 == 0 );
srand(time(NULL));
rx[0] = 0.5 * Lx + 100.0 * 100.0 * a0;
vx[0] = 0.0;
ry[0] = 0.5 * Ly;
vy[0] = sqrt(e*e / (me * 100.0 * 100.0 * a0));
rz[0] = 0.5 * Lz;
vz[0] = 0.0;
m[0] = me; q[0] = -e; type[0] = 1;
rx[1] = 0.5 * Lx;
vx[1] = 0.0;
ry[1] = 0.5 * Ly;
vy[1] = 0.0;
rz[1] = 0.5 * Lz;
vz[1] = 0.0;
m[1] = mp; q[1] = e; type[1] = 2;
U = K = KelXY = KelZ = KpXY = KpZ = 0.0;
for(i = 0; i < N; ++i) {get_kinetic_energy(i);}
update_forces();
E = K + U;
initK = K;
initU = U;
initE = E;
int st = 0;
double t;
double dx, dy, dz;
FILE* f_en = fopen("energy","w");
FILE* f_pos = fopen("part_pos", "w");
get_distance_vector(0, 1, &dx, &dy, &dz);
fprintf(f_pos,"%d\t%g\t%g\t%g\t%g\t%g\n",st, rx[0]/Lx, ry[0]/Ly, rx[1]/Lx, ry[1]/Ly, sqrt(dx*dx + dy*dy + dz*dz));
fprintf(f_en,"%d\t%g\t%g\t%g\n", st, E, initE, (1.0 - fabs(E/initE)));
for(t = 0.0; t < t_max; t += dt)
{
U = K = KelXY = KelZ = KpXY = KpZ = 0.0;
/*VV_step(dt);*/
B_step(dt);
E = U + K;
st++;
get_distance_vector(0, 1, &dx, &dy, &dz);
fprintf(f_pos,"%d\t%g\t%g\t%g\t%g\t%g\n",st, rx[0]/Lx, ry[0]/Ly, rx[1]/Lx, ry[1]/Ly, sqrt(dx*dx + dy*dy + dz*dz));
fprintf(f_en,"%d\t%g\t%g\t%g\n", st, E, initE, (1.0 - fabs(E/initE)));
if(initE != 0.0 && fabs(initE - E) > 0.1 * fabs(initE))
{
fprintf(stderr,"!\n");
return EXIT_FAILURE;
}
}
return EXIT_SUCCESS;
}