103 lines
3.3 KiB
C++
103 lines
3.3 KiB
C++
#include <stdio.h>
|
|
|
|
#include "directxmath/directxmath.h"
|
|
|
|
#include "boids.h"
|
|
|
|
namespace boids {
|
|
|
|
boid_forces forces;
|
|
boid_configuration configuration;
|
|
|
|
static inline void reset_cohesion()
|
|
{
|
|
forces.cohesion.center_of_mass = XMVectorZero();
|
|
forces.cohesion.force = XMVectorZero();
|
|
}
|
|
|
|
static inline void reset_alignment()
|
|
{
|
|
forces.alignment.average_velocity = XMVectorZero();
|
|
forces.alignment.force = XMVectorZero();
|
|
}
|
|
|
|
static inline void reset_separation()
|
|
{
|
|
forces.separation.force = XMVectorZero();
|
|
}
|
|
|
|
static inline bool vector_equal(XMVECTOR V1, XMVECTOR V2)
|
|
{
|
|
uint32_t CR;
|
|
XMVectorEqualR(&CR, V1, V2);
|
|
return XMComparisonAllTrue(CR);
|
|
}
|
|
|
|
static inline XMVECTOR cohesion_force(boid const & this_boid, float const reciprocal_neighbor_count)
|
|
{
|
|
XMVECTOR center_of_mass = forces.cohesion.center_of_mass * reciprocal_neighbor_count;
|
|
XMVECTOR cohesion_vector = center_of_mass - this_boid.position;
|
|
if (vector_equal(cohesion_vector, XMVectorZero()))
|
|
return XMVectorZero();
|
|
XMVECTOR cohesion_direction = XMVector3NormalizeEst(cohesion_vector);
|
|
return cohesion_direction * configuration.cohesion.coefficient;
|
|
}
|
|
|
|
static inline XMVECTOR alignment_force(boid const & this_boid, float const reciprocal_neighbor_count)
|
|
{
|
|
XMVECTOR average_velocity = forces.alignment.average_velocity * reciprocal_neighbor_count;
|
|
XMVECTOR alignment_vector = average_velocity - this_boid.velocity;
|
|
if (vector_equal(alignment_vector, XMVectorZero()))
|
|
return XMVectorZero();
|
|
XMVECTOR alignment_direction = XMVector3NormalizeEst(alignment_vector);
|
|
return alignment_direction * configuration.alignment.coefficient;
|
|
}
|
|
|
|
boid_forces const & evaluate(boid const * const boids, int length, int boid_index)
|
|
{
|
|
forces.vision_neighbor_count = 0;
|
|
|
|
reset_cohesion();
|
|
reset_alignment();
|
|
reset_separation();
|
|
|
|
boid const & this_boid = boids[boid_index];
|
|
for (int i = 0; i < length; i++) {
|
|
if (i == boid_index)
|
|
continue;
|
|
|
|
boid const & other_boid = boids[i];
|
|
|
|
XMVECTOR position_vector = this_boid.position - other_boid.position;
|
|
float position_length = XMVectorGetX(XMVector3LengthEst(position_vector));
|
|
if (position_length < configuration.vision_length) {
|
|
forces.vision_neighbor_count += 1;
|
|
|
|
// cohesion
|
|
forces.cohesion.center_of_mass += other_boid.position;
|
|
|
|
// alignment
|
|
forces.alignment.average_velocity += other_boid.velocity;
|
|
}
|
|
|
|
// separation
|
|
if (vector_equal(position_vector, XMVectorZero()))
|
|
continue;
|
|
if (position_length < configuration.separation.minimum_length) {
|
|
float reciprocal_position_length = 1.0f / position_length;
|
|
//float reciprocal_position_length = 1.0f;
|
|
XMVECTOR separation_direction = position_vector * reciprocal_position_length;
|
|
forces.separation.force += separation_direction * reciprocal_position_length * configuration.separation.coefficient;
|
|
}
|
|
}
|
|
|
|
if (forces.vision_neighbor_count != 0) {
|
|
float reciprocal_vision_neighbor_count = 1.0f / forces.vision_neighbor_count;
|
|
forces.cohesion.force = cohesion_force(this_boid, reciprocal_vision_neighbor_count);
|
|
forces.alignment.force = alignment_force(this_boid, reciprocal_vision_neighbor_count);
|
|
}
|
|
|
|
return forces;
|
|
}
|
|
}
|