text: /*************************************************************************/
  /*  shape_3d_sw.cpp                                                      */
  /*************************************************************************/
  /*                       This file is part of:                           */
  /*                           Valjang Engine                                */
  /*                      http://Valjang.fr                          */
  /*************************************************************************/
  /* Copyright (c) 2007-2020 Valjang.                 */
  /* Copyright (c) 2014-2020 Valjang 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       */
  /* "Software"), to deal in the Software without restriction, including   */
  /* without limitation the rights to use, copy, modify, merge, publish,   */
  /* distribute, sublicense, and/or sell copies of the Software, and to    */
  /* permit persons to whom the Software is furnished to do so, subject to */
  /* the following conditions:                                             */
  /*                                                                       */
  /* The above copyright notice and this permission notice shall be        */
  /* included in all copies or substantial portions of the Software.       */
  /*                                                                       */
  /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
  /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
  /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
  /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
  /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
  /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
  /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
  /*************************************************************************/
  
  #include "shape_3d_sw.h"
  
  #include "core/math/geometry_3d.h"
  #include "core/math/quick_hull.h"
  #include "core/sort_array.h"
  
  #define _POINT_SNAP 0.001953125
  #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002
  #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998
  
  void Shape3DSW::configure(const AABB &p_aabb) {
  	aabb = p_aabb;
  	configured = true;
  	for (Map<ShapeOwner3DSW *, int>::Element *E = owners.front(); E; E = E->next()) {
  		ShapeOwner3DSW *co = (ShapeOwner3DSW *)E->key();
  		co->_shape_changed();
  	}
  }
  
  Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const {
  	Vector3 res;
  	int amnt;
  	get_supports(p_normal, 1, &res, amnt);
  	return res;
  }
  
  void Shape3DSW::add_owner(ShapeOwner3DSW *p_owner) {
  	Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner);
  	if (E) {
  		E->get()++;
  	} else {
  		owners[p_owner] = 1;
  	}
  }
  
  void Shape3DSW::remove_owner(ShapeOwner3DSW *p_owner) {
  	Map<ShapeOwner3DSW *, int>::Element *E = owners.find(p_owner);
  	ERR_FAIL_COND(!E);
  	E->get()--;
  	if (E->get() == 0) {
  		owners.erase(E);
  	}
  }
  
  bool Shape3DSW::is_owner(ShapeOwner3DSW *p_owner) const {
  	return owners.has(p_owner);
  }
  
  const Map<ShapeOwner3DSW *, int> &Shape3DSW::get_owners() const {
  	return owners;
  }
  
  Shape3DSW::Shape3DSW() {
  	custom_bias = 0;
  	configured = false;
  }
  
  Shape3DSW::~Shape3DSW() {
  	ERR_FAIL_COND(owners.size());
  }
  
  Plane PlaneShape3DSW::get_plane() const {
  	return plane;
  }
  
  void PlaneShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
  	// gibberish, a plane is infinity
  	r_min = -1e7;
  	r_max = 1e7;
  }
  
  Vector3 PlaneShape3DSW::get_support(const Vector3 &p_normal) const {
  	return p_normal * 1e15;
  }
  
  bool PlaneShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
  	bool inters = plane.intersects_segment(p_begin, p_end, &r_result);
  	if (inters) {
  		r_normal = plane.normal;
  	}
  	return inters;
  }
  
  bool PlaneShape3DSW::intersect_point(const Vector3 &p_point) const {
  	return plane.distance_to(p_point) < 0;
  }
  
  Vector3 PlaneShape3DSW::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 PlaneShape3DSW::get_moment_of_inertia(real_t p_mass) const {
  	return Vector3(); //wtf
  }
  
  void PlaneShape3DSW::_setup(const Plane &p_plane) {
  	plane = p_plane;
  	configure(AABB(Vector3(-1e4, -1e4, -1e4), Vector3(1e4 * 2, 1e4 * 2, 1e4 * 2)));
  }
  
  void PlaneShape3DSW::set_data(const Variant &p_data) {
  	_setup(p_data);
  }
  
  Variant PlaneShape3DSW::get_data() const {
  	return plane;
  }
  
  PlaneShape3DSW::PlaneShape3DSW() {
  }
  
  //
  
  real_t RayShape3DSW::get_length() const {
  	return length;
  }
  
  bool RayShape3DSW::get_slips_on_slope() const {
  	return slips_on_slope;
  }
  
  void RayShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
  	// don't think this will be even used
  	r_min = 0;
  	r_max = 1;
  }
  
  Vector3 RayShape3DSW::get_support(const Vector3 &p_normal) const {
  	if (p_normal.z > 0) {
  		return Vector3(0, 0, length);
  	} else {
  		return Vector3(0, 0, 0);
  	}
  }
  
  void RayShape3DSW::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_THRESHOLD) {
  		r_amount = 2;
  		r_supports[0] = Vector3(0, 0, 0);
  		r_supports[1] = Vector3(0, 0, length);
  	} else if (p_normal.z > 0) {
  		r_amount = 1;
  		*r_supports = Vector3(0, 0, length);
  	} else {
  		r_amount = 1;
  		*r_supports = Vector3(0, 0, 0);
  	}
  }
  
  bool RayShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
  	return false; //simply not possible
  }
  
  bool RayShape3DSW::intersect_point(const Vector3 &p_point) const {
  	return false; //simply not possible
  }
  
  Vector3 RayShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
  	Vector3 s[2] = {
  		Vector3(0, 0, 0),
  		Vector3(0, 0, length)
  	};
  
  	return Geometry3D::get_closest_point_to_segment(p_point, s);
  }
  
  Vector3 RayShape3DSW::get_moment_of_inertia(real_t p_mass) const {
  	return Vector3();
  }
  
  void RayShape3DSW::_setup(real_t p_length, bool p_slips_on_slope) {
  	length = p_length;
  	slips_on_slope = p_slips_on_slope;
  	configure(AABB(Vector3(0, 0, 0), Vector3(0.1, 0.1, length)));
  }
  
  void RayShape3DSW::set_data(const Variant &p_data) {
  	Dictionary d = p_data;
  	_setup(d["length"], d["slips_on_slope"]);
  }
  
  Variant RayShape3DSW::get_data() const {
  	Dictionary d;
  	d["length"] = length;
  	d["slips_on_slope"] = slips_on_slope;
  	return d;
  }
  
  RayShape3DSW::RayShape3DSW() {
  	length = 1;
  	slips_on_slope = false;
  }
  
  /********** SPHERE *************/
  
  real_t SphereShape3DSW::get_radius() const {
  	return radius;
  }
  
  void SphereShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
  	real_t d = p_normal.dot(p_transform.origin);
  
  	// figure out scale at point
  	Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
  	real_t scale = local_normal.length();
  
  	r_min = d - (radius)*scale;
  	r_max = d + (radius)*scale;
  }
  
  Vector3 SphereShape3DSW::get_support(const Vector3 &p_normal) const {
  	return p_normal * radius;
  }
  
  void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
  	*r_supports = p_normal * radius;
  	r_amount = 1;
  }
  
  bool SphereShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
  	return Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(), radius, &r_result, &r_normal);
  }
  
  bool SphereShape3DSW::intersect_point(const Vector3 &p_point) const {
  	return p_point.length() < radius;
  }
  
  Vector3 SphereShape3DSW::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 SphereShape3DSW::get_moment_of_inertia(real_t p_mass) const {
  	real_t s = 0.4 * p_mass * radius * radius;
  	return Vector3(s, s, s);
  }
  
  void SphereShape3DSW::_setup(real_t p_radius) {
  	radius = p_radius;
  	configure(AABB(Vector3(-radius, -radius, -radius), Vector3(radius * 2.0, radius * 2.0, radius * 2.0)));
  }
  
  void SphereShape3DSW::set_data(const Variant &p_data) {
  	_setup(p_data);
  }
  
  Variant SphereShape3DSW::get_data() const {
  	return radius;
  }
  
  SphereShape3DSW::SphereShape3DSW() {
  	radius = 0;
  }
  
  /********** BOX *************/
  
  void BoxShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
  	// no matter the angle, the box is mirrored anyway
  	Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
  
  	real_t length = local_normal.abs().dot(half_extents);
  	real_t distance = p_normal.dot(p_transform.origin);
  
  	r_min = distance - length;
  	r_max = distance + length;
  }
  
  Vector3 BoxShape3DSW::get_support(const Vector3 &p_normal) const {
  	Vector3 point(
  			(p_normal.x < 0) ? -half_extents.x : half_extents.x,
  			(p_normal.y < 0) ? -half_extents.y : half_extents.y,
  			(p_normal.z < 0) ? -half_extents.z : half_extents.z);
  
  	return point;
  }
  
  void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
  	static const int next[3] = { 1, 2, 0 };
  	static const int next2[3] = { 2, 0, 1 };
  
  	for (int i = 0; i < 3; i++) {
  		Vector3 axis;
  		axis[i] = 1.0;
  		real_t dot = p_normal.dot(axis);
  		if (Math::abs(dot) > _FACE_IS_VALID_SUPPORT_THRESHOLD) {
  			//Vector3 axis_b;
  
  			bool neg = dot < 0;
  			r_amount = 4;
  
  			Vector3 point;
  			point[i] = half_extents[i];
  
  			int i_n = next[i];
  			int i_n2 = next2[i];
  
  			static const real_t sign[4][2] = {
  
  				{ -1.0, 1.0 },
  				{ 1.0, 1.0 },
  				{ 1.0, -1.0 },
  				{ -1.0, -1.0 },
  			};
  
  			for (int j = 0; j < 4; j++) {
  				point[i_n] = sign[j][0] * half_extents[i_n];
  				point[i_n2] = sign[j][1] * half_extents[i_n2];
  				r_supports[j] = neg ? -point : point;
  			}
  
  			if (neg) {
  				SWAP(r_supports[1], r_supports[2]);
  				SWAP(r_supports[0], r_supports[3]);
  			}
  
  			return;
  		}
  
  		r_amount = 0;
  	}
  
  	for (int i = 0; i < 3; i++) {
  		Vector3 axis;
  		axis[i] = 1.0;
  
  		if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
  			r_amount = 2;
  
  			int i_n = next[i];
  			int i_n2 = next2[i];
  
  			Vector3 point = half_extents;
  
  			if (p_normal[i_n] < 0) {
  				point[i_n] = -point[i_n];
  			}
  			if (p_normal[i_n2] < 0) {
  				point[i_n2] = -point[i_n2];
  			}
  
  			r_supports[0] = point;
  			point[i] = -point[i];
  			r_supports[1] = point;
  			return;
  		}
  	}
  	/* USE POINT */
  
  	Vector3 point(
  			(p_normal.x < 0) ? -half_extents.x : half_extents.x,
  			(p_normal.y < 0) ? -half_extents.y : half_extents.y,
  			(p_normal.z < 0) ? -half_extents.z : half_extents.z);
  
  	r_amount = 1;
  	r_supports[0] = point;
  }
  
  bool BoxShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
  	AABB aabb(-half_extents, half_extents * 2.0);
  
  	return aabb.intersects_segment(p_begin, p_end, &r_result, &r_normal);
  }
  
  bool BoxShape3DSW::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 BoxShape3DSW::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 = Geometry3D::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 BoxShape3DSW::get_moment_of_inertia(real_t p_mass) const {
  	real_t lx = half_extents.x;
  	real_t ly = half_extents.y;
  	real_t lz = half_extents.z;
  
  	return Vector3((p_mass / 3.0) * (ly * ly + lz * lz), (p_mass / 3.0) * (lx * lx + lz * lz), (p_mass / 3.0) * (lx * lx + ly * ly));
  }
  
  void BoxShape3DSW::_setup(const Vector3 &p_half_extents) {
  	half_extents = p_half_extents.abs();
  
  	configure(AABB(-half_extents, half_extents * 2));
  }
  
  void BoxShape3DSW::set_data(const Variant &p_data) {
  	_setup(p_data);
  }
  
  Variant BoxShape3DSW::get_data() const {
  	return half_extents;
  }
  
  BoxShape3DSW::BoxShape3DSW() {
  }
  
  /********** CAPSULE *************/
  
  void CapsuleShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
  	Vector3 n = p_transform.basis.xform_inv(p_normal).normalized();
  	real_t h = (n.z > 0) ? height : -height;
  
  	n *= radius;
  	n.z += h * 0.5;
  
  	r_max = p_normal.dot(p_transform.xform(n));
  	r_min = p_normal.dot(p_transform.xform(-n));
  }
  
  Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const {
  	Vector3 n = p_normal;
  
  	real_t h = (n.z > 0) ? height : -height;
  
  	n *= radius;
  	n.z += h * 0.5;
  	return n;
  }
  
  void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
  	Vector3 n = p_normal;
  
  	real_t d = n.z;
  
  	if (Math::abs(d) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
  		// make it flat
  		n.z = 0.0;
  		n.normalize();
  		n *= radius;
  
  		r_amount = 2;
  		r_supports[0] = n;
  		r_supports[0].z += height * 0.5;
  		r_supports[1] = n;
  		r_supports[1].z -= height * 0.5;
  
  	} else {
  		real_t h = (d > 0) ? height : -height;
  
  		n *= radius;
  		n.z += h * 0.5;
  		r_amount = 1;
  		*r_supports = n;
  	}
  }
  
  bool CapsuleShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
  	Vector3 norm = (p_end - p_begin).normalized();
  	real_t min_d = 1e20;
  
  	Vector3 res, n;
  	bool collision = false;
  
  	Vector3 auxres, auxn;
  	bool collided;
  
  	// test against cylinder and spheres :-|
  
  	collided = Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &auxres, &auxn);
  
  	if (collided) {
  		real_t d = norm.dot(auxres);
  		if (d < min_d) {
  			min_d = d;
  			res = auxres;
  			n = auxn;
  			collision = true;
  		}
  	}
  
  	collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * 0.5), radius, &auxres, &auxn);
  
  	if (collided) {
  		real_t d = norm.dot(auxres);
  		if (d < min_d) {
  			min_d = d;
  			res = auxres;
  			n = auxn;
  			collision = true;
  		}
  	}
  
  	collided = Geometry3D::segment_intersects_sphere(p_begin, p_end, Vector3(0, 0, height * -0.5), radius, &auxres, &auxn);
  
  	if (collided) {
  		real_t d = norm.dot(auxres);
  
  		if (d < min_d) {
  			min_d = d;
  			res = auxres;
  			n = auxn;
  			collision = true;
  		}
  	}
  
  	if (collision) {
  		r_result = res;
  		r_normal = n;
  	}
  	return collision;
  }
  
  bool CapsuleShape3DSW::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 CapsuleShape3DSW::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 = Geometry3D::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 CapsuleShape3DSW::get_moment_of_inertia(real_t p_mass) const {
  	// use bad AABB approximation
  	Vector3 extents = get_aabb().size * 0.5;
  
  	return Vector3(
  			(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
  			(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
  			(p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
  }
  
  void CapsuleShape3DSW::_setup(real_t p_height, real_t p_radius) {
  	height = p_height;
  	radius = p_radius;
  	configure(AABB(Vector3(-radius, -radius, -height * 0.5 - radius), Vector3(radius * 2, radius * 2, height + radius * 2.0)));
  }
  
  void CapsuleShape3DSW::set_data(const Variant &p_data) {
  	Dictionary d = p_data;
  	ERR_FAIL_COND(!d.has("radius"));
  	ERR_FAIL_COND(!d.has("height"));
  	_setup(d["height"], d["radius"]);
  }
  
  Variant CapsuleShape3DSW::get_data() const {
  	Dictionary d;
  	d["radius"] = radius;
  	d["height"] = height;
  	return d;
  }
  
  CapsuleShape3DSW::CapsuleShape3DSW() {
  	height = radius = 0;
  }
  
  /********** CONVEX POLYGON *************/
  
  void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
  	int vertex_count = mesh.vertices.size();
  	if (vertex_count == 0) {
  		return;
  	}
  
  	const Vector3 *vrts = &mesh.vertices[0];
  
  	for (int i = 0; i < vertex_count; i++) {
  		real_t d = p_normal.dot(p_transform.xform(vrts[i]));
  
  		if (i == 0 || d > r_max) {
  			r_max = d;
  		}
  		if (i == 0 || d < r_min) {
  			r_min = d;
  		}
  	}
  }
  
  Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const {
  	Vector3 n = p_normal;
  
  	int vert_support_idx = -1;
  	real_t support_max = 0;
  
  	int vertex_count = mesh.vertices.size();
  	if (vertex_count == 0) {
  		return Vector3();
  	}
  
  	const Vector3 *vrts = &mesh.vertices[0];
  
  	for (int i = 0; i < vertex_count; i++) {
  		real_t d = n.dot(vrts[i]);
  
  		if (i == 0 || d > support_max) {
  			support_max = d;
  			vert_support_idx = i;
  		}
  	}
  
  	return vrts[vert_support_idx];
  }
  
  void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
  	const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
  	int fc = mesh.faces.size();
  
  	const Geometry3D::MeshData::Edge *edges = mesh.edges.ptr();
  	int ec = mesh.edges.size();
  
  	const Vector3 *vertices = mesh.vertices.ptr();
  	int vc = mesh.vertices.size();
  
  	//find vertex first
  	real_t max = 0;
  	int vtx = 0;
  
  	for (int i = 0; i < vc; i++) {
  		real_t d = p_normal.dot(vertices[i]);
  
  		if (i == 0 || d > max) {
  			max = d;
  			vtx = i;
  		}
  	}
  
  	for (int i = 0; i < fc; i++) {
  		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();
  
  			bool valid = false;
  			for (int j = 0; j < ic; j++) {
  				if (ind[j] == vtx) {
  					valid = true;
  					break;
  				}
  			}
  
  			if (!valid) {
  				continue;
  			}
  
  			int m = MIN(p_max, ic);
  			for (int j = 0; j < m; j++) {
  				r_supports[j] = vertices[ind[j]];
  			}
  			r_amount = m;
  			return;
  		}
  	}
  
  	for (int i = 0; i < ec; i++) {
  		real_t dot = (vertices[edges[i].a] - vertices[edges[i].b]).normalized().dot(p_normal);
  		dot = ABS(dot);
  		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];
  			r_supports[1] = vertices[edges[i].b];
  			return;
  		}
  	}
  
  	r_supports[0] = vertices[vtx];
  	r_amount = 1;
  }
  
  bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
  	const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
  	int fc = mesh.faces.size();
  
  	const Vector3 *vertices = mesh.vertices.ptr();
  
  	Vector3 n = p_end - p_begin;
  	real_t min = 1e20;
  	bool col = false;
  
  	for (int i = 0; i < fc; i++) {
  		if (faces[i].plane.normal.dot(n) > 0) {
  			continue; //opposing face
  		}
  
  		int ic = faces[i].indices.size();
  		const int *ind = faces[i].indices.ptr();
  
  		for (int j = 1; j < ic - 1; j++) {
  			Face3 f(vertices[ind[0]], vertices[ind[j]], vertices[ind[j + 1]]);
  			Vector3 result;
  			if (f.intersects_segment(p_begin, p_end, &result)) {
  				real_t d = n.dot(result);
  				if (d < min) {
  					min = d;
  					r_result = result;
  					r_normal = faces[i].plane.normal;
  					col = true;
  				}
  
  				break;
  			}
  		}
  	}
  
  	return col;
  }
  
  bool ConvexPolygonShape3DSW::intersect_point(const Vector3 &p_point) const {
  	const Geometry3D::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 ConvexPolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
  	const Geometry3D::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 Geometry3D::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 = Geometry3D::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 ConvexPolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const {
  	// use bad AABB approximation
  	Vector3 extents = get_aabb().size * 0.5;
  
  	return Vector3(
  			(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
  			(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
  			(p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
  }
  
  void ConvexPolygonShape3DSW::_setup(const Vector<Vector3> &p_vertices) {
  	Error err = QuickHull::build(p_vertices, mesh);
  	if (err != OK) {
  		ERR_PRINT("Failed to build QuickHull");
  	}
  
  	AABB _aabb;
  
  	for (int i = 0; i < mesh.vertices.size(); i++) {
  		if (i == 0) {
  			_aabb.position = mesh.vertices[i];
  		} else {
  			_aabb.expand_to(mesh.vertices[i]);
  		}
  	}
  
  	configure(_aabb);
  }
  
  void ConvexPolygonShape3DSW::set_data(const Variant &p_data) {
  	_setup(p_data);
  }
  
  Variant ConvexPolygonShape3DSW::get_data() const {
  	return mesh.vertices;
  }
  
  ConvexPolygonShape3DSW::ConvexPolygonShape3DSW() {
  }
  
  /********** FACE POLYGON *************/
  
  void FaceShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
  	for (int i = 0; i < 3; i++) {
  		Vector3 v = p_transform.xform(vertex[i]);
  		real_t d = p_normal.dot(v);
  
  		if (i == 0 || d > r_max) {
  			r_max = d;
  		}
  
  		if (i == 0 || d < r_min) {
  			r_min = d;
  		}
  	}
  }
  
  Vector3 FaceShape3DSW::get_support(const Vector3 &p_normal) const {
  	int vert_support_idx = -1;
  	real_t support_max = 0;
  
  	for (int i = 0; i < 3; i++) {
  		real_t d = p_normal.dot(vertex[i]);
  
  		if (i == 0 || d > support_max) {
  			support_max = d;
  			vert_support_idx = i;
  		}
  	}
  
  	return vertex[vert_support_idx];
  }
  
  void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
  	Vector3 n = p_normal;
  
  	/** TEST FACE AS SUPPORT **/
  	if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) {
  		r_amount = 3;
  		for (int i = 0; i < 3; i++) {
  			r_supports[i] = vertex[i];
  		}
  		return;
  	}
  
  	/** FIND SUPPORT VERTEX **/
  
  	int vert_support_idx = -1;
  	real_t support_max = 0;
  
  	for (int i = 0; i < 3; i++) {
  		real_t d = n.dot(vertex[i]);
  
  		if (i == 0 || d > support_max) {
  			support_max = d;
  			vert_support_idx = i;
  		}
  	}
  
  	/** TEST EDGES AS SUPPORT **/
  
  	for (int i = 0; i < 3; i++) {
  		int nx = (i + 1) % 3;
  		if (i != vert_support_idx && nx != vert_support_idx) {
  			continue;
  		}
  
  		// 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_THRESHOLD) {
  			r_amount = 2;
  			r_supports[0] = vertex[i];
  			r_supports[1] = vertex[nx];
  			return;
  		}
  	}
  
  	r_amount = 1;
  	r_supports[0] = vertex[vert_support_idx];
  }
  
  bool FaceShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
  	bool c = Geometry3D::segment_intersects_triangle(p_begin, p_end, vertex[0], vertex[1], vertex[2], &r_result);
  	if (c) {
  		r_normal = Plane(vertex[0], vertex[1], vertex[2]).normal;
  		if (r_normal.dot(p_end - p_begin) > 0) {
  			r_normal = -r_normal;
  		}
  	}
  
  	return c;
  }
  
  bool FaceShape3DSW::intersect_point(const Vector3 &p_point) const {
  	return false; //face is flat
  }
  
  Vector3 FaceShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
  	return Face3(vertex[0], vertex[1], vertex[2]).get_closest_point_to(p_point);
  }
  
  Vector3 FaceShape3DSW::get_moment_of_inertia(real_t p_mass) const {
  	return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
  }
  
  FaceShape3DSW::FaceShape3DSW() {
  	configure(AABB());
  }
  
  Vector<Vector3> ConcavePolygonShape3DSW::get_faces() const {
  	Vector<Vector3> rfaces;
  	rfaces.resize(faces.size() * 3);
  
  	for (int i = 0; i < faces.size(); i++) {
  		Face f = faces.get(i);
  
  		for (int j = 0; j < 3; j++) {
  			rfaces.set(i * 3 + j, vertices.get(f.indices[j]));
  		}
  	}
  
  	return rfaces;
  }
  
  void ConcavePolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
  	int count = vertices.size();
  	if (count == 0) {
  		r_min = 0;
  		r_max = 0;
  		return;
  	}
  	const Vector3 *vptr = vertices.ptr();
  
  	for (int i = 0; i < count; i++) {
  		real_t d = p_normal.dot(p_transform.xform(vptr[i]));
  
  		if (i == 0 || d > r_max) {
  			r_max = d;
  		}
  		if (i == 0 || d < r_min) {
  			r_min = d;
  		}
  	}
  }
  
  Vector3 ConcavePolygonShape3DSW::get_support(const Vector3 &p_normal) const {
  	int count = vertices.size();
  	if (count == 0) {
  		return Vector3();
  	}
  
  	const Vector3 *vptr = vertices.ptr();
  
  	Vector3 n = p_normal;
  
  	int vert_support_idx = -1;
  	real_t support_max = 0;
  
  	for (int i = 0; i < count; i++) {
  		real_t d = n.dot(vptr[i]);
  
  		if (i == 0 || d > support_max) {
  			support_max = d;
  			vert_support_idx = i;
  		}
  	}
  
  	return vptr[vert_support_idx];
  }
  
  void ConcavePolygonShape3DSW::_cull_segment(int p_idx, _SegmentCullParams *p_params) const {
  	const BVH *bvh = &p_params->bvh[p_idx];
  
  	/*
  	if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d)
  		return; //test against whole AABB, which isn't very costly
  	*/
  
  	//printf("addr: %p\n",bvh);
  	if (!bvh->aabb.intersects_segment(p_params->from, p_params->to)) {
  		return;
  	}
  
  	if (bvh->face_index >= 0) {
  		Vector3 res;
  		Vector3 vertices[3] = {
  			p_params->vertices[p_params->faces[bvh->face_index].indices[0]],
  			p_params->vertices[p_params->faces[bvh->face_index].indices[1]],
  			p_params->vertices[p_params->faces[bvh->face_index].indices[2]]
  		};
  
  		if (Geometry3D::segment_intersects_triangle(
  					p_params->from,
  					p_params->to,
  					vertices[0],
  					vertices[1],
  					vertices[2],
  					&res)) {
  			real_t d = p_params->dir.dot(res) - p_params->dir.dot(p_params->from);
  			//TODO, seems segmen/triangle intersection is broken :(
  			if (d > 0 && d < p_params->min_d) {
  				p_params->min_d = d;
  				p_params->result = res;
  				p_params->normal = Plane(vertices[0], vertices[1], vertices[2]).normal;
  				p_params->collisions++;
  			}
  		}
  
  	} else {
  		if (bvh->left >= 0) {
  			_cull_segment(bvh->left, p_params);
  		}
  		if (bvh->right >= 0) {
  			_cull_segment(bvh->right, p_params);
  		}
  	}
  }
  
  bool ConcavePolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
  	if (faces.size() == 0) {
  		return false;
  	}
  
  	// unlock data
  	const Face *fr = faces.ptr();
  	const Vector3 *vr = vertices.ptr();
  	const BVH *br = bvh.ptr();
  
  	_SegmentCullParams params;
  	params.from = p_begin;
  	params.to = p_end;
  	params.collisions = 0;
  	params.dir = (p_end - p_begin).normalized();
  
  	params.faces = fr;
  	params.vertices = vr;
  	params.bvh = br;
  
  	params.min_d = 1e20;
  	// cull
  	_cull_segment(0, &params);
  
  	if (params.collisions > 0) {
  		r_result = params.result;
  		r_normal = params.normal;
  		return true;
  	} else {
  		return false;
  	}
  }
  
  bool ConcavePolygonShape3DSW::intersect_point(const Vector3 &p_point) const {
  	return false; //face is flat
  }
  
  Vector3 ConcavePolygonShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
  	return Vector3();
  }
  
  void ConcavePolygonShape3DSW::_cull(int p_idx, _CullParams *p_params) const {
  	const BVH *bvh = &p_params->bvh[p_idx];
  
  	if (!p_params->aabb.intersects(bvh->aabb)) {
  		return;
  	}
  
  	if (bvh->face_index >= 0) {
  		const Face *f = &p_params->faces[bvh->face_index];
  		FaceShape3DSW *face = p_params->face;
  		face->normal = f->normal;
  		face->vertex[0] = p_params->vertices[f->indices[0]];
  		face->vertex[1] = p_params->vertices[f->indices[1]];
  		face->vertex[2] = p_params->vertices[f->indices[2]];
  		p_params->callback(p_params->userdata, face);
  
  	} else {
  		if (bvh->left >= 0) {
  			_cull(bvh->left, p_params);
  		}
  
  		if (bvh->right >= 0) {
  			_cull(bvh->right, p_params);
  		}
  	}
  }
  
  void ConcavePolygonShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const {
  	// make matrix local to concave
  	if (faces.size() == 0) {
  		return;
  	}
  
  	AABB local_aabb = p_local_aabb;
  
  	// unlock data
  	const Face *fr = faces.ptr();
  	const Vector3 *vr = vertices.ptr();
  	const BVH *br = bvh.ptr();
  
  	FaceShape3DSW face; // use this to send in the callback
  
  	_CullParams params;
  	params.aabb = local_aabb;
  	params.face = &face;
  	params.faces = fr;
  	params.vertices = vr;
  	params.bvh = br;
  	params.callback = p_callback;
  	params.userdata = p_userdata;
  
  	// cull
  	_cull(0, &params);
  }
  
  Vector3 ConcavePolygonShape3DSW::get_moment_of_inertia(real_t p_mass) const {
  	// use bad AABB approximation
  	Vector3 extents = get_aabb().size * 0.5;
  
  	return Vector3(
  			(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
  			(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
  			(p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
  }
  
  struct _VolumeSW_BVH_Element {
  	AABB aabb;
  	Vector3 center;
  	int face_index;
  };
  
  struct _VolumeSW_BVH_CompareX {
  	_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
  		return a.center.x < b.center.x;
  	}
  };
  
  struct _VolumeSW_BVH_CompareY {
  	_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
  		return a.center.y < b.center.y;
  	}
  };
  
  struct _VolumeSW_BVH_CompareZ {
  	_FORCE_INLINE_ bool operator()(const _VolumeSW_BVH_Element &a, const _VolumeSW_BVH_Element &b) const {
  		return a.center.z < b.center.z;
  	}
  };
  
  struct _VolumeSW_BVH {
  	AABB aabb;
  	_VolumeSW_BVH *left;
  	_VolumeSW_BVH *right;
  
  	int face_index;
  };
  
  _VolumeSW_BVH *_volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements, int p_size, int &count) {
  	_VolumeSW_BVH *bvh = memnew(_VolumeSW_BVH);
  
  	if (p_size == 1) {
  		//leaf
  		bvh->aabb = p_elements[0].aabb;
  		bvh->left = nullptr;
  		bvh->right = nullptr;
  		bvh->face_index = p_elements->face_index;
  		count++;
  		return bvh;
  	} else {
  		bvh->face_index = -1;
  	}
  
  	AABB aabb;
  	for (int i = 0; i < p_size; i++) {
  		if (i == 0) {
  			aabb = p_elements[i].aabb;
  		} else {
  			aabb.merge_with(p_elements[i].aabb);
  		}
  	}
  	bvh->aabb = aabb;
  	switch (aabb.get_longest_axis_index()) {
  		case 0: {
  			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareX> sort_x;
  			sort_x.sort(p_elements, p_size);
  
  		} break;
  		case 1: {
  			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareY> sort_y;
  			sort_y.sort(p_elements, p_size);
  		} break;
  		case 2: {
  			SortArray<_VolumeSW_BVH_Element, _VolumeSW_BVH_CompareZ> sort_z;
  			sort_z.sort(p_elements, p_size);
  		} break;
  	}
  
  	int split = p_size / 2;
  	bvh->left = _volume_sw_build_bvh(p_elements, split, count);
  	bvh->right = _volume_sw_build_bvh(&p_elements[split], p_size - split, count);
  
  	//printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
  	count++;
  	return bvh;
  }
  
  void ConcavePolygonShape3DSW::_fill_bvh(_VolumeSW_BVH *p_bvh_tree, BVH *p_bvh_array, int &p_idx) {
  	int idx = p_idx;
  
  	p_bvh_array[idx].aabb = p_bvh_tree->aabb;
  	p_bvh_array[idx].face_index = p_bvh_tree->face_index;
  	//printf("%p - %i: %i(%p)  -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right);
  
  	if (p_bvh_tree->left) {
  		p_bvh_array[idx].left = ++p_idx;
  		_fill_bvh(p_bvh_tree->left, p_bvh_array, p_idx);
  
  	} else {
  		p_bvh_array[p_idx].left = -1;
  	}
  
  	if (p_bvh_tree->right) {
  		p_bvh_array[idx].right = ++p_idx;
  		_fill_bvh(p_bvh_tree->right, p_bvh_array, p_idx);
  
  	} else {
  		p_bvh_array[p_idx].right = -1;
  	}
  
  	memdelete(p_bvh_tree);
  }
  
  void ConcavePolygonShape3DSW::_setup(Vector<Vector3> p_faces) {
  	int src_face_count = p_faces.size();
  	if (src_face_count == 0) {
  		configure(AABB());
  		return;
  	}
  	ERR_FAIL_COND(src_face_count % 3);
  	src_face_count /= 3;
  
  	const Vector3 *facesr = p_faces.ptr();
  
  	Vector<_VolumeSW_BVH_Element> bvh_array;
  	bvh_array.resize(src_face_count);
  
  	_VolumeSW_BVH_Element *bvh_arrayw = bvh_array.ptrw();
  
  	faces.resize(src_face_count);
  	Face *facesw = faces.ptrw();
  
  	vertices.resize(src_face_count * 3);
  
  	Vector3 *verticesw = vertices.ptrw();
  
  	AABB _aabb;
  
  	for (int i = 0; i < src_face_count; i++) {
  		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.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;
  		facesw[i].indices[2] = i * 3 + 2;
  		facesw[i].normal = face.get_plane().normal;
  		verticesw[i * 3 + 0] = face.vertex[0];
  		verticesw[i * 3 + 1] = face.vertex[1];
  		verticesw[i * 3 + 2] = face.vertex[2];
  		if (i == 0) {
  			_aabb = bvh_arrayw[i].aabb;
  		} else {
  			_aabb.merge_with(bvh_arrayw[i].aabb);
  		}
  	}
  
  	int count = 0;
  	_VolumeSW_BVH *bvh_tree = _volume_sw_build_bvh(bvh_arrayw, src_face_count, count);
  
  	bvh.resize(count + 1);
  
  	BVH *bvh_arrayw2 = bvh.ptrw();
  
  	int idx = 0;
  	_fill_bvh(bvh_tree, bvh_arrayw2, idx);
  
  	configure(_aabb); // this type of shape has no margin
  }
  
  void ConcavePolygonShape3DSW::set_data(const Variant &p_data) {
  	_setup(p_data);
  }
  
  Variant ConcavePolygonShape3DSW::get_data() const {
  	return get_faces();
  }
  
  ConcavePolygonShape3DSW::ConcavePolygonShape3DSW() {
  }
  
  /* HEIGHT MAP SHAPE */
  
  Vector<real_t> HeightMapShape3DSW::get_heights() const {
  	return heights;
  }
  
  int HeightMapShape3DSW::get_width() const {
  	return width;
  }
  
  int HeightMapShape3DSW::get_depth() const {
  	return depth;
  }
  
  real_t HeightMapShape3DSW::get_cell_size() const {
  	return cell_size;
  }
  
  void HeightMapShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
  	//not very useful, but not very used either
  	p_transform.xform(get_aabb()).project_range_in_plane(Plane(p_normal, 0), r_min, r_max);
  }
  
  Vector3 HeightMapShape3DSW::get_support(const Vector3 &p_normal) const {
  	//not very useful, but not very used either
  	return get_aabb().get_support(p_normal);
  }
  
  bool HeightMapShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const {
  	return false;
  }
  
  bool HeightMapShape3DSW::intersect_point(const Vector3 &p_point) const {
  	return false;
  }
  
  Vector3 HeightMapShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
  	return Vector3();
  }
  
  void HeightMapShape3DSW::cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const {
  }
  
  Vector3 HeightMapShape3DSW::get_moment_of_inertia(real_t p_mass) const {
  	// use bad AABB approximation
  	Vector3 extents = get_aabb().size * 0.5;
  
  	return Vector3(
  			(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
  			(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
  			(p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
  }
  
  void HeightMapShape3DSW::_setup(Vector<real_t> p_heights, int p_width, int p_depth, real_t p_cell_size) {
  	heights = p_heights;
  	width = p_width;
  	depth = p_depth;
  	cell_size = p_cell_size;
  
  	const real_t *r = heights.ptr();
  
  	AABB aabb;
  
  	for (int i = 0; i < depth; i++) {
  		for (int j = 0; j < width; j++) {
  			real_t h = r[i * width + j];
  
  			Vector3 pos(j * cell_size, h, i * cell_size);
  			if (i == 0 || j == 0) {
  				aabb.position = pos;
  			} else {
  				aabb.expand_to(pos);
  			}
  		}
  	}
  
  	configure(aabb);
  }
  
  void HeightMapShape3DSW::set_data(const Variant &p_data) {
  	ERR_FAIL_COND(p_data.get_type() != Variant::DICTIONARY);
  	Dictionary d = p_data;
  	ERR_FAIL_COND(!d.has("width"));
  	ERR_FAIL_COND(!d.has("depth"));
  	ERR_FAIL_COND(!d.has("cell_size"));
  	ERR_FAIL_COND(!d.has("heights"));
  
  	int width = d["width"];
  	int depth = d["depth"];
  	real_t cell_size = d["cell_size"];
  	Vector<real_t> heights = d["heights"];
  
  	ERR_FAIL_COND(width <= 0);
  	ERR_FAIL_COND(depth <= 0);
  	ERR_FAIL_COND(cell_size <= CMP_EPSILON);
  	ERR_FAIL_COND(heights.size() != (width * depth));
  	_setup(heights, width, depth, cell_size);
  }
  
  Variant HeightMapShape3DSW::get_data() const {
  	ERR_FAIL_V(Variant());
  }
  
  HeightMapShape3DSW::HeightMapShape3DSW() {
  	width = 0;
  	depth = 0;
  	cell_size = 0;
  }
  