bool collides( SphereBody& body1, SphereBody& body2, real_t collision_damping )
{
// Velocity of body1 relative to body2.
Vector3 v1 = body1.velocity - body2.velocity;
Vector3 distance_vec = body2.position - body1.position;
// If the velocity is too low or in the opposite direction, they do not
// collide.
if (squared_length(v1) > 1e-8 && dot(v1, distance_vec) > 0) {
real_t distance = length(distance_vec);
// The distance between them is less than the sum of their radiuses.
if (distance < (body1.radius + body2.radius)) {
Vector3 v2 = Vector3::Zero();
Vector3 d_vec = distance_vec / distance;
// Compute new velocities.
v2 = body2.velocity + 2 * d_vec * body1.mass / (body2.mass + body1.mass) * dot(v1, d_vec);
body1.velocity = body1.velocity + body2.mass / body1.mass * (body2.velocity - v2);
body2.velocity = v2;
// Apply damping.
body1.velocity = (1 - collision_damping) * body1.velocity;
body2.velocity = (1 - collision_damping) * body2.velocity;
return true;
}
}
return false;
}
/**
* Convert the projection of sphere center to barycentric coordinate.
* Check whether this point is within the triangle surface.
*/
bool barycentric(const Vector3 projection, const TriangleBody body) {
Vector3 normal = cross(body.vertices[1] - body.vertices[0],
body.vertices[2] - body.vertices[0]);
Vector3 nb = cross(body.vertices[0] - body.vertices[2],
projection - body.vertices[2]);
Vector3 nc = cross(body.vertices[1] - body.vertices[0],
projection - body.vertices[0]);
real_t beta = dot(normal, nb) / squared_length(normal);
if (beta < 0 || beta > 1)
return false;
real_t gamma = dot(normal, nc) / squared_length(normal);
if (gamma < 0 || gamma > 1 - beta)
return false;
return true;
}
/**
* Check whether the sphere hits the triangle's vertices or edges.
*/
bool collides_vertices_axes(const Vector3 center, const real_t radius, const TriangleBody body) {
// Check vertices.
for (size_t i = 0; i < 3; i++) {
if (squared_length(center - body.vertices[i]) < radius * radius)
return true;
}
// Check edges.
for (size_t i = 0; i < 3; i++) {
Vector3 edge = normalize(body.vertices[(i + 1) % 3] - body.vertices[i]);
Vector3 projection = dot(center - body.vertices[i], edge) * edge +
body.vertices[i];
if (squared_length(center - projection) < radius * radius)
return true;
}
return false;
}
bool cylinder::is_inside (const vec3f& p) const
{
auto d (pt2_ - pt1_);
auto pd (p - pt1_);
auto dot (dot_prod(pd, d));
if (dot < 0.0f || dot > sqlength_)
return false;
return (squared_length(pd) - square(dot) / sqlength_) < sqradius_;
}
void SphereBody::apply_force( const Vector3& f, const Vector3& offset )
{
Vector3 fDir = cross(offset, f);
if ( (offset == Vector3::Zero) || (fDir == Vector3::Zero) ) {
this->force += f;
}
else {
//Split force into 2 components.
Vector3 parr = this->position_phy - offset;
this->force += ( dot(f, offset) / squared_length(offset) ) * offset;
// (0.4 * this->mass * this->radius * this->radius);
}
}
void compare_mesh(const Mesh& mesh_1, const Mesh& mesh_2)
{
vertex_iterator it_1,end_1, it_2, end_2;
CGAL::cpp11::tie(it_1, end_1) = vertices(mesh_1);
CGAL::cpp11::tie(it_2, end_2) = vertices(mesh_2);
boost::property_map<Mesh, boost::vertex_point_t>::type
ppmap_1 = get(boost::vertex_point, mesh_1), ppmap_2 = get(boost::vertex_point, mesh_2);
Point total_dif(0,0,0);
for( ; it_1 != end_1; ++it_1 , ++it_2)
{
total_dif = total_dif + (get(ppmap_1, *it_1) - get(ppmap_2, *it_2));
}
const double n = static_cast<double>(num_vertices(mesh_1));
double average_mesh_dif = squared_length(total_dif) / n / n;
std::cerr << "Average mesh difference: " << average_mesh_dif << std::endl;
assert( average_mesh_dif < squared_threshold);
}
bool collides( SphereBody& body1, PlaneBody& body2, real_t collision_damping )
{
real_t v_projection= dot(body1.velocity, body2.normal);
// Get the normal vector in the direction facing the sphere.
Vector3 normal_facing_sphere = (dot(body1.position - body2.position, body2.normal) > 0) ?
body2.normal : -body2.normal;
// If the velocity is large enough and the sphere is moving toward the plane.
if (squared_length(body1.velocity) > 1e-8 &&
(dot(body1.velocity, normal_facing_sphere)) < 0) {
real_t distance = std::abs(dot(body2.normal, body1.position - body2.position));
if (distance < body1.radius) {
// Reverse the velocity component perpendicular to the plane.
body1.velocity = body1.velocity - 2 * v_projection * body2.normal;
body1.velocity = (1 - collision_damping) * body1.velocity;
return true;
}
}
return false;
}
/*
void Spring::step( real_t dt )
{
Vector3 f1 = -constant * (body1_offset) - damping * (body1->position / dt);
body1->apply_force(f1, Vector3::Zero());
Vector3 f2 = -constant * body2_offset - damping * (body2->position / dt);
body2->apply_force(f2, Vector3::Zero());
}
*/
void Spring::step( real_t dt )
{
Vector3 b1_attach = this->body1->position + this->body1_offset;
Vector3 b2_attach = this->body2->position + this->body2_offset;
real_t fLen = length(b1_attach - b2_attach);
real_t x = fLen - this->equilibrium;
Vector3 normDir = (b2_attach - b1_attach) / fLen;
Vector3 velComp = normDir * x * dot(normalize(this->body1->velocity), normDir);
Vector3 SpringForce = (normDir * x) * this->constant;// - this->damping * velComp;
Vector3 Torque = cross( (b2_attach - this->body1->position), this->constant * ( (this->body2->position + this->body2_offset) - b2_attach) );
this->body2->angular_velocity += Torque;
if (squared_length(SpringForce) <= 1.0f)
SpringForce = Vector3::Zero();
this->body1->apply_force(SpringForce, Vector3::Zero());
// this->body1->apply_force(Torque, this->body2_offset);
}
double squared_distance (const Vector3& v, const Vector3& u) {
return squared_length(v - u);
}
float vec2d::length(void) const
{
return sqrt(squared_length());
}
inline const double squared_distance(const wfpos& a, const wfpos& b)
{
return squared_length(a.relative_to(b));
}
inline const double squared_length(const wfvec& v)
{
return squared_length(v.double_vec());
}
void Physics::step( real_t dt )
{
std::vector< SphereBody* >::iterator it;
std::vector< SphereBody* >::iterator it2;
std::vector< PlaneBody* >::iterator pt;
std::vector< TriangleBody* >::iterator tt;
for (it = spheres.begin(); it != spheres.end(); ++it)
{
SphereBody *sb = *it;
for (tt = triangles.begin(); tt != triangles.end(); ++tt)
{
if (collides(*sb, **tt, collision_damping))
{
Vector3 n =
normalize(
cross(
(*tt)->vertices[0] - (*tt)->vertices[1],
(*tt)->vertices[1] - (*tt)->vertices[2]
)
);
Vector3 u = sb->velocity - 2 * dot(sb->velocity, n) * n;
sb->velocity = u - (collision_damping * u);
if (squared_length(sb->velocity) <= 1.0f)
sb->velocity = Vector3::Zero();
}
}
for (it2 = spheres.begin(); it2 != spheres.end(); ++it2)
{
if (collides(**it, **it2, collision_damping) && *it != *it2)
{
SphereBody *sb2 = *it2;
Vector3 v1 = sb->velocity - sb2->velocity;
Vector3 d = ((sb2->position - sb->position)
/ distance(sb->position, sb2->position));
Vector3 v22 = 2 * d
* (sb->mass / (sb->mass + sb2->mass))
* (dot(v1, d));
Vector3 u2 = sb2->velocity + v22;
Vector3 u1 =
((sb->mass * sb->velocity)
+ (sb2->mass * sb2->velocity)
- (sb2->mass * u2))
/ sb->mass;
sb->velocity = u1 - collision_damping * u1;
sb2->velocity = u2 - collision_damping * u2;
if (squared_length(sb->velocity) <= 1.0f)
sb->velocity = Vector3::Zero();
if (squared_length(sb2->velocity) <= 1.0f)
sb2->velocity = Vector3::Zero();
}
}
for (pt = planes.begin(); pt != planes.end(); ++pt)
{
if (collides(*sb, **pt, collision_damping))
{
Vector3 u = sb->velocity - 2 * dot(sb->velocity, (*pt)->normal) * (*pt)->normal;
sb->velocity = u - collision_damping * u;
if (squared_length(sb->velocity) <= 1.0f)
sb->velocity = Vector3::Zero();
}
}
sb->force = Vector3::Zero();
for (SpringList::iterator si = springs.begin(); si != springs.end(); ++si) {
(*si)->step(dt);
}
sb->apply_force(gravity, Vector3::Zero());
// si pas de force et velocite, ne pas faire les tests
State initMove = {sb->position, sb->velocity, sb->force / sb->mass};
State s = rk4(dt, initMove);
sb->position += s.x;
sb->sphere->position = sb->position;
sb->velocity += s.v;
real_t i = (2.0f / 5.0f) * sb->mass * (sb->radius * sb->radius);
Vector3 angular_accel = sb->torque / i;
State initRot = {Vector3::Zero(), sb->angular_velocity, angular_accel};
s = rk4(dt, initRot);
if (s.x != Vector3::Zero())
{
Quaternion q(s.x, length(s.x));
Quaternion qq = q * sb->orientation;
sb->orientation = qq;
sb->sphere->orientation = sb->orientation;
}
}
}
void radiosity_lightmap::generate(world_lightmap_access& data,
const chunk_coordinates& pos,
const surface& s, lightmap_hr &lightchunk,
unsigned int phase) const
{
std::array<float, 6> irr_sun, irr_ambient, irr_artificial;
vector half {0.5f, 0.5f, 0.5f};
auto lmi = std::begin(lightchunk);
for (const faces& f : s)
{
fill(irr_sun, 0.f);
fill(irr_ambient, 0.f);
fill(irr_artificial, 0.f);
auto surr (surroundings(f.pos, phase + 1));
for (auto sp : surr)
{
if (sp == f.pos)
continue;
auto found (find(s, sp));
if (found == s.end())
continue;
auto& other (*found);
for (int i = 0; i < 6; ++i)
{
if (!f[i])
continue;
vector this_face (half + f.pos + vector(dir_vector[i]) * 0.52f);
for (int j = 0; j < 6; ++j)
{
if (i == j || !other[j])
continue;
vector that_face (half + other.pos + vector(dir_vector[j])* 0.52f);
vector conn (that_face - this_face);
vector norm_conn (normalize(conn));
float dp1 (dot_prod<vector>(dir_vector[j], -norm_conn));
if (dp1 <= 0)
continue;
float dp2 = dot_prod<vector>(dir_vector[i], norm_conn);
if (dp2 <= 0)
continue;
float intensity = dp1 * dp2 / squared_length(conn);
auto dist = std::distance(std::begin(s), found);
auto& olm = lightchunk.data[dist];
irr_sun[i] += olm.sunlight * intensity;
irr_ambient[i] += olm.ambient * intensity;
irr_artificial[i] += olm.artificial * intensity;
}
}
}
for (int i (0); i < 6; ++i)
{
if (!f[i])
continue;
lmi->r_sunlight = irr_sun[i] * 255.f + 0.49f;
lmi->r_ambient = irr_ambient[i] * 255.f + 0.49f;
lmi->r_artificial = irr_artificial[i] * 255.f + 0.49f;
++lmi;
}
}
}
/**
* Efficiency function: does not require square root operation.
*/
inline real_t squared_distance( const Vector4& lhs, const Vector4& rhs ) {
return squared_length( lhs - rhs );
}
const t project_vector (const t& a, const t& b)
{
assert(b != t::zero());
return dot_prod(a, b) / squared_length(b) * b;
}
const double squared_distance (const t& lhs, const t& rhs)
{
return squared_length(lhs - rhs);
}
const double length (const t& v)
{
return std::sqrt(squared_length(v));
}
double length (const Vector3& v) {
return sqrt(squared_length(v));
}
/**
* Returns the length of a vector.
*/
inline real_t length( const Vector4& v ) {
return sqrt( squared_length( v ) );
}
static int vector3_squared_length(lua_State* L)
{
LuaStack stack(L);
stack.push_float(squared_length(stack.get_vector3(1)));
return 1;
}
