void *compute_next_dynamics(void *arg)
{
struct Work_Unit *chunk = (struct Work_Unit *)arg;
// For each object...
for( int object_i = chunk->start_index; object_i < chunk->stop_index; ++object_i ) {
Vector3 total_force = { 0.0, 0.0, 0.0 };
// Consider interactions with all other objects...
for( int object_j = 0; object_j < OBJECT_COUNT; ++object_j ) {
if( object_i == object_j ) continue;
Vector3 displacement = v3_subtract(
current_dynamics[object_j].position, current_dynamics[object_i].position );
double distance_squared = magnitude_squared( displacement );
double distance = sqrt( distance_squared );
double force_magnitude =
( G * object_array[object_i].mass * object_array[object_j].mass ) / distance_squared;
Vector3 force = v3_multiply( (force_magnitude / distance ), displacement );
total_force = v3_add( total_force, force );
}
// Total force on object_i is now known. Compute acceleration, velocity and position.
Vector3 acceleration = v3_divide( total_force, object_array[object_i].mass );
Vector3 delta_v = v3_multiply( TIME_STEP, acceleration );
Vector3 delta_position = v3_multiply( TIME_STEP, current_dynamics[object_i].velocity );
next_dynamics[object_i].velocity =
v3_add( current_dynamics[object_i].velocity, delta_v );
next_dynamics[object_i].position =
v3_add( current_dynamics[object_i].position, delta_position );
}
return NULL;
}
void BasebandProcessor::update_spectrum() {
// Called from idle thread (after EVT_MASK_SPECTRUM is flagged)
if( channel_spectrum_request_update ) {
/* Decimated buffer is full. Compute spectrum. */
std::array<std::complex<float>, 256> samples_swapped;
fft_swap(channel_spectrum, samples_swapped);
channel_spectrum_request_update = false;
fft_c_preswapped(samples_swapped);
ChannelSpectrumMessage spectrum_message;
for(size_t i=0; i<spectrum_message.spectrum.db.size(); i++) {
const auto mag2 = magnitude_squared(samples_swapped[i]);
const float db = complex16_mag_squared_to_dbv_norm(mag2);
constexpr float mag_scale = 5.0f;
const unsigned int v = (db * mag_scale) + 255.0f;
spectrum_message.spectrum.db[i] = std::max(0U, std::min(255U, v));
}
/* TODO: Rename .db -> .magnitude, or something more (less!) accurate. */
spectrum_message.spectrum.db_count = spectrum_message.spectrum.db.size();
spectrum_message.spectrum.sampling_rate = channel_spectrum_sampling_rate;
spectrum_message.spectrum.channel_filter_pass_frequency = channel_filter_pass_frequency;
spectrum_message.spectrum.channel_filter_stop_frequency = channel_filter_stop_frequency;
shared_memory.application_queue.push(spectrum_message);
}
}
void DrawPlane(const AABB& bounds, const Plane& plane)
{
Vec3 points[6];
struct Edge
{
int start;
int end;
};
static Edge edges[12] =
{
// bottom
{0, 1},
{1, 3},
{3, 2},
{2, 0},
// top
{4, 5},
{5, 7},
{7, 6},
{6, 4},
// top to bottom sides
{0, 4},
{1, 5},
{2, 6},
{3, 7},
};
// clip plane to bounds and render a quad
Vec3 corners[8];
const Vec3 *minmax[2] = { &bounds.m_min, &bounds.m_max };
for(int j = 0; j < 8; ++j)
{
int iz = j & 1;
int ix = (j >> 1) & 1;
int iy = (j >> 2) & 1;
corners[j] = Vec3(minmax[ix]->x, minmax[iy]->y, minmax[iz]->z);
}
int numPoints = 0;
// add corners
for(int j = 0; j < 8; ++j)
{
float planeDist = dot(plane.m_normal, corners[j]) - plane.m_d;
if(fabs(planeDist) < EPSILON)
points[numPoints++] = corners[j];
}
// add edges
for(int j = 0; j < 12; ++j)
{
Vec3 a = corners[edges[j].start];
Vec3 b = corners[edges[j].end];
Vec3 ab = b - a;
float t = (plane.m_d - dot(plane.m_normal, a)) / dot(plane.m_normal, ab);
if(t >= 0.f && t <= 1.f)
{
Vec3 pt = a + t * ab;
Vec3 ptA = a - pt;
Vec3 ptB = b - pt;
float distSqA = magnitude_squared(ptA);
float distSqB = magnitude_squared(ptB);
if(distSqA > EPSILON_SQ && distSqB > EPSILON_SQ)
{
points[numPoints++] = pt;
if(numPoints == 6)
break;
}
}
}
if(numPoints < 3)
return;
// Sort results
float inv_num = 1.f / numPoints;
Vec3 center(0,0,0);
for(int j = 0; j < numPoints; ++j)
center += points[j] * inv_num;
Vec3 sideVec = normalize(points[0] - center);
Vec3 upVec = normalize(cross(plane.m_normal, sideVec));
for(int j = 1; j < numPoints; ++j)
{
Vec3 toPointJ = points[j] - center;
float angleJ = AngleWrap(atan2(dot(upVec, toPointJ), dot(sideVec, toPointJ)));
for(int k = j+1; k < numPoints; ++k)
{
Vec3 toPointK = points[k] - center;
float angleK = AngleWrap(atan2(dot(upVec, toPointK), dot(sideVec, toPointK)));
if(angleK < angleJ)
{
angleJ = angleK;
Vec3 temp = points[j];
points[j] = points[k];
points[k] = temp;
}
}
}
// Draw outline
glColor3f(7.f, 0.3f, 0.3f);
glLineWidth(2.f);
glBegin(GL_LINES);
for(int j = 0; j < numPoints; ++j)
{
int next = (j + 1) % numPoints;
glVertex3fv(&points[j].x);
glVertex3fv(&points[next].x);
}
glEnd();
glLineWidth(1.f);
// Draw triangles
glColor4f(7.f, 0.3f, 0.3f, 0.3f);
glBegin(GL_TRIANGLE_FAN);
glVertex3fv(¢er.x);
for(int j = 0; j < numPoints; ++j)
glVertex3fv(&points[j].x);
glVertex3fv(&points[0].x);
glEnd();
glBegin(GL_TRIANGLE_FAN);
glVertex3fv(¢er.x);
for(int j = numPoints-1; j >= 0; --j)
glVertex3fv(&points[j].x);
glVertex3fv(&points[numPoints-1].x);
glEnd();
}
float magnitude(const vec3& v) {
return sqrt(magnitude_squared(v));
}
float ivec2::magnitude() const {
return std::sqrt(static_cast<float>(magnitude_squared()));
}
float vec2::magnitude() const {
return std::sqrt(magnitude_squared());
}
