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; }