diff options
Diffstat (limited to 'servers/physics/shape_sw.cpp')
| -rw-r--r-- | servers/physics/shape_sw.cpp | 256 |
1 files changed, 243 insertions, 13 deletions
diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp index 4ce716c70a..b4004c8c94 100644 --- a/servers/physics/shape_sw.cpp +++ b/servers/physics/shape_sw.cpp @@ -6,6 +6,7 @@ /* http://www.godotengine.org */ /*************************************************************************/ /* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md) */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ @@ -31,8 +32,8 @@ #include "quick_hull.h" #include "sort.h" #define _POINT_SNAP 0.001953125 -#define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.0002 -#define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.9998 +#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002 +#define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998 void ShapeSW::configure(const Rect3 &p_aabb) { aabb = p_aabb; @@ -116,6 +117,20 @@ bool PlaneShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_en return inters; } +bool PlaneShapeSW::intersect_point(const Vector3 &p_point) const { + + return plane.distance_to(p_point) < 0; +} + +Vector3 PlaneShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + if (plane.is_point_over(p_point)) { + return plane.project(p_point); + } else { + return p_point; + } +} + Vector3 PlaneShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); //wtf @@ -164,7 +179,7 @@ Vector3 RayShapeSW::get_support(const Vector3 &p_normal) const { void RayShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { - if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { + if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; r_supports[0] = Vector3(0, 0, 0); @@ -183,6 +198,21 @@ bool RayShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, return false; //simply not possible } +bool RayShapeSW::intersect_point(const Vector3 &p_point) const { + + return false; //simply not possible +} + +Vector3 RayShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + Vector3 s[2] = { + Vector3(0, 0, 0), + Vector3(0, 0, length) + }; + + return Geometry::get_closest_point_to_segment(p_point, s); +} + Vector3 RayShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); @@ -244,6 +274,20 @@ bool SphereShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_e return Geometry::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal); } +bool SphereShapeSW::intersect_point(const Vector3 &p_point) const { + + return p_point.length() < radius; +} + +Vector3 SphereShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + Vector3 p = p_point; + float l = p.length(); + if (l < radius) + return p_point; + return (p / l) * radius; +} + Vector3 SphereShapeSW::get_moment_of_inertia(real_t p_mass) const { real_t s = 0.4 * p_mass * radius * radius; @@ -305,7 +349,7 @@ void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_sup Vector3 axis; axis[i] = 1.0; real_t dot = p_normal.dot(axis); - if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_TRESHOLD) { + if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { //Vector3 axis_b; @@ -349,7 +393,7 @@ void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_sup Vector3 axis; axis[i] = 1.0; - if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { + if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; @@ -389,6 +433,62 @@ bool BoxShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal); } +bool BoxShapeSW::intersect_point(const Vector3 &p_point) const { + + return (Math::abs(p_point.x) < half_extents.x && Math::abs(p_point.y) < half_extents.y && Math::abs(p_point.z) < half_extents.z); +} + +Vector3 BoxShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + int outside = 0; + Vector3 min_point; + + for (int i = 0; i < 3; i++) { + + if (Math::abs(p_point[i]) > half_extents[i]) { + outside++; + if (outside == 1) { + //use plane if only one side matches + Vector3 n; + n[i] = SGN(p_point[i]); + + Plane p(n, half_extents[i]); + min_point = p.project(p_point); + } + } + } + + if (!outside) + return p_point; //it's inside, don't do anything else + + if (outside == 1) //if only above one plane, this plane clearly wins + return min_point; + + //check segments + float min_distance = 1e20; + Vector3 closest_vertex = half_extents * p_point.sign(); + Vector3 s[2] = { + closest_vertex, + closest_vertex + }; + + for (int i = 0; i < 3; i++) { + + s[1] = closest_vertex; + s[1][i] = -s[1][i]; //edge + + Vector3 closest_edge = Geometry::get_closest_point_to_segment(p_point, s); + + float d = p_point.distance_to(closest_edge); + if (d < min_distance) { + min_point = closest_edge; + min_distance = d; + } + } + + return min_point; +} + Vector3 BoxShapeSW::get_moment_of_inertia(real_t p_mass) const { real_t lx = half_extents.x; @@ -459,7 +559,7 @@ void CapsuleShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r real_t d = n.z; - if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { + if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { // make it flat n.z = 0.0; @@ -541,6 +641,32 @@ bool CapsuleShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_ return collision; } +bool CapsuleShapeSW::intersect_point(const Vector3 &p_point) const { + + if (Math::abs(p_point.z) < height * 0.5) { + return Vector3(p_point.x, p_point.y, 0).length() < radius; + } else { + Vector3 p = p_point; + p.z = Math::abs(p.z) - height * 0.5; + return p.length() < radius; + } +} + +Vector3 CapsuleShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + Vector3 s[2] = { + Vector3(0, 0, -height * 0.5), + Vector3(0, 0, height * 0.5), + }; + + Vector3 p = Geometry::get_closest_point_to_segment(p_point, s); + + if (p.distance_to(p_point) < radius) + return p_point; + + return p + (p_point - p).normalized() * radius; +} + Vector3 CapsuleShapeSW::get_moment_of_inertia(real_t p_mass) const { // use crappy AABB approximation @@ -654,7 +780,7 @@ void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vect for (int i = 0; i < fc; i++) { - if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_TRESHOLD) { + if (faces[i].plane.normal.dot(p_normal) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { int ic = faces[i].indices.size(); const int *ind = faces[i].indices.ptr(); @@ -684,7 +810,7 @@ void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vect real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal); dot = ABS(dot); - if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { + if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) { r_amount = 2; r_supports[0] = vertices[edges[i].a]; @@ -737,6 +863,81 @@ bool ConvexPolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vecto return col; } +bool ConvexPolygonShapeSW::intersect_point(const Vector3 &p_point) const { + + const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + + for (int i = 0; i < fc; i++) { + + if (faces[i].plane.distance_to(p_point) >= 0) + return false; + } + + return true; +} + +Vector3 ConvexPolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + int fc = mesh.faces.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + + bool all_inside = true; + for (int i = 0; i < fc; i++) { + + if (!faces[i].plane.is_point_over(p_point)) + continue; + + all_inside = false; + bool is_inside = true; + int ic = faces[i].indices.size(); + const int *indices = faces[i].indices.ptr(); + + for (int j = 0; j < ic; j++) { + + Vector3 a = vertices[indices[j]]; + Vector3 b = vertices[indices[(j + 1) % ic]]; + Vector3 n = (a - b).cross(faces[i].plane.normal).normalized(); + if (Plane(a, n).is_point_over(p_point)) { + is_inside = false; + break; + } + } + + if (is_inside) { + return faces[i].plane.project(p_point); + } + } + + if (all_inside) { + return p_point; + } + + float min_distance = 1e20; + Vector3 min_point; + + //check edges + const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + int ec = mesh.edges.size(); + for (int i = 0; i < ec; i++) { + + Vector3 s[2] = { + vertices[edges[i].a], + vertices[edges[i].b] + }; + + Vector3 closest = Geometry::get_closest_point_to_segment(p_point, s); + float d = closest.distance_to(p_point); + if (d < min_distance) { + min_distance = d; + min_point = closest; + } + } + + return min_point; +} + Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(real_t p_mass) const { // use crappy AABB approximation @@ -756,7 +957,7 @@ void ConvexPolygonShapeSW::_setup(const Vector<Vector3> &p_vertices) { for (int i = 0; i < mesh.vertices.size(); i++) { if (i == 0) - _aabb.pos = mesh.vertices[i]; + _aabb.position = mesh.vertices[i]; else _aabb.expand_to(mesh.vertices[i]); } @@ -817,7 +1018,7 @@ void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_su Vector3 n = p_normal; /** TEST FACE AS SUPPORT **/ - if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) { + if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 3; for (int i = 0; i < 3; i++) { @@ -853,7 +1054,7 @@ void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_su // check if edge is valid as a support real_t dot = (vertex[i] - vertex[nx]).normalized().dot(n); dot = ABS(dot); - if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) { + if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) { r_amount = 2; r_supports[0] = vertex[i]; @@ -879,6 +1080,16 @@ bool FaceShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end return c; } +bool FaceShapeSW::intersect_point(const Vector3 &p_point) const { + + return false; //face is flat +} + +Vector3 FaceShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point); +} + Vector3 FaceShapeSW::get_moment_of_inertia(real_t p_mass) const { return Vector3(); // Sorry, but i don't think anyone cares, FaceShape! @@ -1045,6 +1256,16 @@ bool ConcavePolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vect } } +bool ConcavePolygonShapeSW::intersect_point(const Vector3 &p_point) const { + + return false; //face is flat +} + +Vector3 ConcavePolygonShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + return Vector3(); +} + void ConcavePolygonShapeSW::_cull(int p_idx, _CullParams *p_params) const { const BVH *bvh = &p_params->bvh[p_idx]; @@ -1387,7 +1608,7 @@ void ConcavePolygonShapeSW::_setup(PoolVector<Vector3> p_faces) { Face3 face(facesr[i * 3 + 0], facesr[i * 3 + 1], facesr[i * 3 + 2]); bvh_arrayw[i].aabb = face.get_aabb(); - bvh_arrayw[i].center = bvh_arrayw[i].aabb.pos + bvh_arrayw[i].aabb.size * 0.5; + bvh_arrayw[i].center = bvh_arrayw[i].aabb.position + bvh_arrayw[i].aabb.size * 0.5; bvh_arrayw[i].face_index = i; facesw[i].indices[0] = i * 3 + 0; facesw[i].indices[1] = i * 3 + 1; @@ -1470,6 +1691,15 @@ bool HeightMapShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 & return false; } +bool HeightMapShapeSW::intersect_point(const Vector3 &p_point) const { + return false; +} + +Vector3 HeightMapShapeSW::get_closest_point_to(const Vector3 &p_point) const { + + return Vector3(); +} + void HeightMapShapeSW::cull(const Rect3 &p_local_aabb, Callback p_callback, void *p_userdata) const { } @@ -1503,7 +1733,7 @@ void HeightMapShapeSW::_setup(PoolVector<real_t> p_heights, int p_width, int p_d Vector3 pos(j * cell_size, h, i * cell_size); if (i == 0 || j == 0) - aabb.pos = pos; + aabb.position = pos; else aabb.expand_to(pos); } |