stable, pre-major changes

Pavel Govyadinov
1 parent 035d798f
Showing 3 changed files with 339 additions and 255 deletions Show diff stats
stim/gl/gl_spider.h
stim/math/plane.h
stim/math/rect.h
@@ -81,8 +81,8 @@ class gl_spider : public virtual gl_texture&lt;T&gt;
 		int numSamplesPos;
 		int numSamplesMag;
-//		float stepsize = 4.0;			//Step size.
-		float stepsize = 3.0;			//Step size.
+		float stepsize = 5.0;			//Step size.
+//		float stepsize = 3.0;			//Step size.
 		int current_cost;			//variable to store the cost of the current step.
@@ -140,11 +140,12 @@ class gl_spider : public virtual gl_texture&lt;T&gt;
 			setMatrix();			//create the transformation matrix.
 			glCallList(dList+1);		//move the templates to p, d, m.
 			int best = getCost(ptexbufferID, numSamplesPos);		//find min cost.
+			std::cerr << best << std::endl;
 			stim::vec<float> next(		//find next position.
- 			pV[best][0],
-			pV[best][1],
-			pV[best][2],
-			1);
+ 				pV[best][0],
+				pV[best][1],
+				pV[best][2],
+				1);
 			next = cT*next;			//find next position.
 			setPosition(
 					next[0]*S[0]*R[0],
@@ -727,7 +728,7 @@ class gl_spider : public virtual gl_texture&lt;T&gt;
 		///Best results if samples is can create a perfect root.
 		///Default Constructor
 		gl_spider
-		(int samples = 1089, int samplespos = 400,int samplesmag = 144)
+		(int samples = 1089, int samplespos = 441,int samplesmag = 144)
 		{
 			p = vec<float>(0.0, 0.0, 0.0);
 			d = vec<float>(0.0, 0.0, 1.0);
@@ -750,7 +751,7 @@ class gl_spider : public virtual gl_texture&lt;T&gt;
 		///@param int 	samples, number of templates this spider is going to use.
 		gl_spider
 		(float pos_x, float pos_y, float pos_z, float dir_x, float dir_y, float dir_z,
-			float mag_x, int numsamples = 1089, int numsamplespos = 400, int numsamplesmag =144)
+			float mag_x, int numsamples = 1089, int numsamplespos = 441, int numsamplesmag =144)
 		{
 			p = vec<float>(pos_x, pos_y, pos_z);
 			d = vec<float>(dir_x, dir_y, dir_z);
@@ -768,7 +769,7 @@ class gl_spider : public virtual gl_texture&lt;T&gt;
 		///@param float mag, size of the vector.
 		///@param int 	samples, number of templates this spider is going to use.
 		gl_spider
-		(stim::vec<float> pos, stim::vec<float> dir, float mag, int samples = 1089, int samplesPos = 400, int samplesMag = 144)
+		(stim::vec<float> pos, stim::vec<float> dir, float mag, int samples = 1089, int samplesPos = 441, int samplesMag = 144)
 		{
 			p = pos;
 			d = dir;
@@ -815,13 +816,13 @@ class gl_spider : public virtual gl_texture&lt;T&gt;
 			glListBase(dList);
 			Bind(texbufferID, fboID, numSamples);
 				genDirectionVectors(5*M_PI/4);
-//			Unbind();
-			Bind(ptexbufferID, bfboID, numSamplesPos);
+			Unbind();
+			Bind(ptexbufferID, pfboID, numSamplesPos);
 				genPositionVectors();
-//			Unbind();
+			Unbind();
 			Bind(mtexbufferID, mfboID, numSamplesMag);
 				genMagnitudeVectors();
-//			Unbind();
+			Unbind();
 			Bind(btexbufferID, bfboID, 27);
 			DrawCylinder();
 			Unbind();
@@ -1150,13 +1151,13 @@ class gl_spider : public virtual gl_texture&lt;T&gt;
 				start = std::clock();
 			#endif
 				findOptimalDirection();
-			//Unbind();
+			Unbind();
 			Bind(ptexbufferID, pfboID, numSamplesPos);
 				findOptimalPosition();
-			//Unbind();
+			Unbind();
 			Bind(mtexbufferID, mfboID, numSamplesMag);
 				findOptimalScale();
-			//Unbind();
+			Unbind();
 			CHECK_OPENGL_ERROR
 			#ifdef TESTING
@@ -1566,7 +1567,9 @@ class gl_spider : public virtual gl_texture&lt;T&gt;
 						}
 						else {  
 							cL.push_back(stim::vec<float>(p[0], p[1],p[2]));
-							cM.push_back(m[0]);
+							cM.push_back(stim::vec<float>(m[0], m[0]));
+//							cM.push_back(m[0]);
+
 							sk.TexCoord(m[0]);
 							sk.Vertex(p[0], p[1], p[2]);
 							Bind(btexbufferID, bfboID, 27);
-#ifndef RTS_PLANE_H
-#define RTS_PLANE_H
+#ifndef STIM_PLANE_H
+#define STIM_PLANE_H
 #include <iostream>
 #include <stim/math/vector.h>
-#include "rts/cuda/callable.h"
+#include <stim/cuda/cudatools/callable.h>
+#include <stim/math/quaternion.h>
-namespace stim{
-template <typename T, int D> class plane;
+namespace stim
+{
+template<typename T> class plane;
 }
-template <typename T, int D>
-CUDA_CALLABLE stim::plane<T, D> operator-(stim::plane<T, D> v);
-
-namespace stim{
-
-template <class T, int D = 3>
-class plane{
-
-	//a plane is defined by a point and a normal
-
-private:
-
-	vec<T, D> P;	//point on the plane
-	vec<T, D> N;	//plane normal
-
-	CUDA_CALLABLE void init(){
-		P = vec<T, D>(0, 0, 0);
-		N = vec<T, D>(0, 0, 1);
-	}
-
-
-public:
-
-	//default constructor
-	CUDA_CALLABLE plane(){
-		init();
-	}
+template<typename T>
+CUDA_CALLABLE stim::plane<T> operator-(stim::plane<T> v);
+
+namespace stim
+{
+
+template <typename T>
+class plane
+{
+	protected:
+		stim::vec<T> P;
+		stim::vec<T> N;
+		stim::vec<T> U;
+
+		///Initializes the plane with standard coordinates.
+		///
+		CUDA_CALLABLE void init()
+		{
+			P = stim::vec<T>(0, 0, 0);
+			N = stim::vec<T>(0, 0, 1);
+			U = stim::vec<T>(1, 0, 0);
+		}
+
+	public:
-	CUDA_CALLABLE plane(vec<T, D> n, vec<T, D> p = vec<T, D>(0, 0, 0)){
-		P = p;
-		N = n.norm();
-	}
-
-	CUDA_CALLABLE plane(T z_pos){
-		init();
-		P[2] = z_pos;
-	}
-
-	//create a plane from three points (a triangle)
-	CUDA_CALLABLE plane(vec<T, D> a, vec<T, D> b, vec<T, D> c){
-		P = c;
-		N = (c - a).cross(b - a);
-		if(N.len() == 0)	//handle the degenerate case when two vectors are the same, N = 0
-			N = 0;
-		else
-			N = N.norm();
-	}
-
-	template< typename U >
-	CUDA_CALLABLE operator plane<U, D>(){
-
-		plane<U, D> result(N, P);
-		return result;
-	}
-
-	CUDA_CALLABLE vec<T, D> norm(){
-		return N;
-	}
-
-	CUDA_CALLABLE vec<T, D> p(){
-		return P;
-	}
-
-	//flip the plane front-to-back
-	CUDA_CALLABLE plane<T, D> flip(){
-		plane<T, D> result = *this;
-		result.N = -result.N;
-		return result;
-	}
-
-	//determines how a vector v intersects the plane (1 = intersects front, 0 = within plane, -1 = intersects back)
-	CUDA_CALLABLE int face(vec<T, D> v){
-		
-		T dprod = v.dot(N);		//get the dot product between v and N
-
-		//conditional returns the appropriate value
-		if(dprod < 0)
-			return 1;
-		else if(dprod > 0)
-			return -1;
-		else
-			return 0;
-	}
-
-	//determine on which side of the plane a point lies (1 = front, 0 = on the plane, -1 = back)
-	CUDA_CALLABLE int side(vec<T, D> p){
-
-		vec<T, D> v = p - P;	//get the vector from P to the query point p
-
-		return face(v);
-	}
-
-	//compute the component of v that is perpendicular to the plane
-	CUDA_CALLABLE vec<T, D> perpendicular(vec<T, D> v){
-		return N * v.dot(N);
-	}
-
-	//compute the projection of v in the plane
-	CUDA_CALLABLE vec<T, D> parallel(vec<T, D> v){
-		return v - perpendicular(v);
-	}
-
-	CUDA_CALLABLE void decompose(vec<T, D> v, vec<T, D>& para, vec<T, D>& perp){
-		perp = N * v.dot(N);
-		para = v - perp;
-	}
-
-	//get both the parallel and perpendicular components of a vector v w.r.t. the plane
-	CUDA_CALLABLE void project(vec<T, D> v, vec<T, D> &v_par, vec<T, D> &v_perp){
-
-		v_perp = v.dot(N);
-		v_par = v - v_perp;
-	}
-
-	//compute the reflection of v off of the plane
-	CUDA_CALLABLE vec<T, D> reflect(vec<T, D> v){
-
-		//compute the reflection using N_prime as the plane normal
-		vec<T, D> par = parallel(v);
-		vec<T, D> r = (-v) + par * 2;
-
-		/*std::cout<<"----------------REFLECT-----------------------------"<<std::endl;
-		std::cout<<str()<<std::endl;
-		std::cout<<"v: "<<v<<std::endl;
-		std::cout<<"r: "<<r<<std::endl;
-		std::cout<<"Perpendicular: "<<perpendicular(v)<<std::endl;
-		std::cout<<"Parallel: "<<par<<std::endl;*/
-		return r;
-
-	}
-
-	CUDA_CALLABLE rts::plane<T, D> operator-()
-	{
-		rts::plane<T, D> p = *this;
-
-		//negate the normal vector
-		p.N = -p.N;
-
-		return p;
-	}
-
-	//output a string
-	std::string str(){
-		std::stringstream ss;
-		ss<<"P: "<<P<<std::endl;
-		ss<<"N: "<<N;
-		return ss.str();
-	}
-
-	///////Friendship
-	//friend CUDA_CALLABLE rts::plane<T, D> operator- <> (rts::plane<T, D> v);
-
-
+		CUDA_CALLABLE plane()
+		{
+			init();
+		}
+
+		CUDA_CALLABLE plane(vec<T> n, vec<T> p = vec<T>(0, 0, 0))
+		{
+			init();
+			P = p;
+			rotate(n.norm());
+		}
+
+		CUDA_CALLABLE plane(T z_pos)
+		{
+			init();
+			P[2] = z_pos;
+		}
+
+		//create a plane from three points (a triangle)
+		CUDA_CALLABLE plane(vec<T> a, vec<T> b, vec<T> c)
+		{
+			init();
+			P = c;
+			stim::vec<T> n = (c - a).cross(b - a);
+			try
+			{
+				if(n.len() != 0)
+				{
+					rotate(n.norm());
+				} else {
+				 	throw 42;
+				}
+			}
+			catch(int i)
+			{
+				std::cerr << "No plane can be creates as all points a,b,c lie on a straight line" << std::endl;
+			}  
+		}
+	
+		template< typename U >
+		CUDA_CALLABLE operator plane<U>()
+		{
+			plane<U> result(N, P);
+			return result;
+
+		}
+
+		CUDA_CALLABLE vec<T> n()
+		{
+			return N;
+		}
+
+		CUDA_CALLABLE vec<T> p()
+		{
+			return P;
+		}
+
+		CUDA_CALLABLE vec<T> u()
+		{
+			return U;
+		}
+
+		///flip the plane front-to-back
+		CUDA_CALLABLE plane<T> flip(){
+			plane<T> result = *this;
+			result.N = -result.N;
+			return result;
+		}
+
+		//determines how a vector v intersects the plane (1 = intersects front, 0 = within plane,     -1 = intersects back)
+		CUDA_CALLABLE int face(vec<T> v){
+			
+			T dprod = v.dot(N);             //get the dot product between v and N
+
+			//conditional returns the appropriate value
+			if(dprod < 0)
+				return 1;
+			else if(dprod > 0)
+				return -1;
+			else
+				return 0;
+		}
+
+		//determine on which side of the plane a point lies (1 = front, 0 = on the plane, -1 = bac    k)
+		CUDA_CALLABLE int side(vec<T> p){
+
+			vec<T> v = p - P;    //get the vector from P to the query point p
+
+			return face(v);
+		}
+
+		//compute the component of v that is perpendicular to the plane
+		CUDA_CALLABLE vec<T> perpendicular(vec<T> v){
+			return N * v.dot(N);
+		}
+
+		//compute the projection of v in the plane
+		CUDA_CALLABLE vec<T> parallel(vec<T> v){
+			return v - perpendicular(v);
+		}
+
+		CUDA_CALLABLE void setU(vec<T> v)
+		{
+			U = (parallel(v.norm())).norm();		
+		}
+
+		CUDA_CALLABLE void decompose(vec<T> v, vec<T>& para, vec<T>& perp){
+			perp = N * v.dot(N);
+			para = v - perp;
+		}
+
+		//get both the parallel and perpendicular components of a vector v w.r.t. the plane
+		CUDA_CALLABLE void project(vec<T> v, vec<T> &v_par, vec<T> &v_perp){
+
+			v_perp = v.dot(N);
+			v_par = v - v_perp;
+		}
+
+		//compute the reflection of v off of the plane
+		CUDA_CALLABLE vec<T> reflect(vec<T> v){
+
+			//compute the reflection using N_prime as the plane normal
+			vec<T> par = parallel(v);
+			vec<T> r = (-v) + par * 2;
+			return r;
+
+		}
+
+		CUDA_CALLABLE stim::plane<T> operator-()
+		{
+			stim::plane<T> p = *this;
+
+			//negate the normal vector
+			p.N = -p.N;
+			return p;
+		}
+
+		//output a string
+		std::string str(){
+			std::stringstream ss;
+			ss<<"P: "<<P<<std::endl;
+			ss<<"N: "<<N<<std::endl;
+			ss<<"U: "<<U;
+			return ss.str();
+		}
+
+
+		CUDA_CALLABLE void rotate(vec<T> n)
+		{
+			quaternion<T> q;
+			q.CreateRotation(N, n);
+			
+			N = q.toMatrix3() * N;
+			U = q.toMatrix3() * U;
+
+		}
+
+		CUDA_CALLABLE void rotate(vec<T> n, vec<T> &X, vec<T> &Y)
+		{
+			quaternion<T> q;
+			q.CreateRotation(N, n);
+			
+			N = q.toMatrix3() * N;
+			U = q.toMatrix3() * U;
+			X = q.toMatrix3() * X;
+			Y = q.toMatrix3() * Y;
+
+		}
 };
-
+		
+		
 }
-
-//arithmetic operators
-
-//negative operator flips the plane (front to back)
-//template <typename T, int D>
-
-
-
-
 #endif
-#ifndef RTS_RECT_H
-#define RTS_RECT_H
+#ifndef STIM_RECT_H
+#define STIM_RECT_H
+
 //enable CUDA_CALLABLE macro
 #include <stim/cuda/cudatools/callable.h>
+#include <stim/math/plane.h>
 #include <stim/math/vector.h>
 #include <stim/math/triangle.h>
-#include <stim/math/quaternion.h>
 #include <iostream>
 #include <iomanip>
 #include <algorithm>
+#include <assert.h>
 namespace stim{
 //template for a rectangle class in ND space
-template <class T>
-struct rect
+template <typename T>
+class rect : plane <T>
 {
 	/*
 		^                   O
 		|                   
 		|                   
-		Y         C         
+		Y         P         
 		|                   
 		|                   
 		O---------X--------->
@@ -28,106 +30,143 @@ struct rect
 private:
-	stim::vec<T> C;
 	stim::vec<T> X;
 	stim::vec<T> Y;
-	CUDA_CALLABLE void scale(T factor){
-		X *= factor;
-		Y *= factor;
-	}
-	CUDA_CALLABLE void normal(vec<T> n){		//orient the rectangle along the specified normal
-
-		n = n.norm();								//normalize, just in case
-		vec<T> n_current = X.cross(Y).norm();	//compute the current normal
-		quaternion<T> q;							//create a quaternion
-		q.CreateRotation(n_current, n);				//initialize a rotation from n_current to n
-
-		//apply the quaternion to the vectors and position
-		X = q.toMatrix3() * X;
-		Y = q.toMatrix3() * Y;
-	}
-
-	CUDA_CALLABLE void init(){
-		C = vec<T>(0, 0, 0);
-		X = vec<T>(1, 0, 0);
-		Y = vec<T>(0, 1, 0);
-	}
 public:
-	CUDA_CALLABLE rect(){
+	using stim::plane<T>::n;
+	using stim::plane<T>::P;
+	using stim::plane<T>::N;
+	using stim::plane<T>::U;
+	using stim::plane<T>::rotate;
+
+	///base constructor.
+	CUDA_CALLABLE rect()
+	 : plane<T>()
+	{
 		init();
 	}
-	//create a rectangle given a size and position
-	CUDA_CALLABLE rect(T size, T z_pos = (T)0){
+	///create a rectangle given a size and position in Z space.
+	///@param size: size of the rectangle in ND space.
+	///@param z_pos z coordinate of the rectangle.
+	CUDA_CALLABLE rect(T size, T z_pos = (T)0)
+	 : plane<T>(z_pos)
+	{
 		init();			//use the default setup
 		scale(size);	//scale the rectangle
-		C[2] = z_pos;
 	}
-	//create a rectangle from a center point, normal, and size
-	CUDA_CALLABLE rect(vec<T> c, vec<T> n = vec<T>(0, 0, 1)){
+	///create a rectangle from a center point, normal
+	///@param c: x,y,z location of the center.
+	///@param n: x,y,z direction of the normal.
+	CUDA_CALLABLE rect(vec<T> c, vec<T> n = vec<T>(0, 0, 1))
+		: plane<T>()
+	{
 		init();			//start with the default setting
-		C = c;
 		normal(n);		//orient
 	}
+	///create a rectangle from a center point, normal, and size
+	///@param c: x,y,z location of the center.
+	///@param s: size of the rectangle.
+	///@param n: x,y,z direction of the normal.
+	CUDA_CALLABLE rect(vec<T> c, T s, vec<T> n = vec<T>(0, 0, 1))
+		: plane<T>()
+	{
+		init();			//start with the default setting
+		scale(s);
+		center(c);
+		rotate(n, X, Y);
+	}
+
+	///creates a rectangle from a centerpoint and an X and Y direction vectors.
+	///@param center: x,y,z location of the center.
+	///@param directionX: u,v,w direction of the X vector.
+	///@param directionY: u,v,w direction of the Y vector.
 	CUDA_CALLABLE rect(vec<T> center, vec<T> directionX, vec<T> directionY )
+		 : plane<T>((directionX.cross(directionY)).norm(),center)
 	{
-		C = center;
 		X = directionX;
 		Y = directionY;
 	}
+	///creates a rectangle from a size, centerpoint, X, and Y direction vectors.
+	///@param size of the rectangle in ND space.
+	///@param center: x,y,z location of the center.
+	///@param directionX: u,v,w direction of the X vector.
+	///@param directionY: u,v,w direction of the Y vector.
 	CUDA_CALLABLE rect(T size, vec<T> center, vec<T> directionX, vec<T> directionY )
+		: plane<T>((directionX.cross(directionY)).norm(),center)
 	{	
-		C = center;
 		X = directionX;
 		Y = directionY;
 		scale(size);
 	}
-
-	CUDA_CALLABLE rect(vec<T> size, vec<T> center, vec<T> directionX, vec<T> directionY )
+	
+	///creates a rectangle from a size, centerpoint, X, and Y direction vectors.
+	///@param size of the rectangle in ND space, size[0] = size in X, size[1] = size in Y.
+	///@param center: x,y,z location of the center.
+	///@param directionX: u,v,w direction of the X vector.
+	///@param directionY: u,v,w direction of the Y vector.
+	CUDA_CALLABLE rect(vec<T> size, vec<T> center, vec<T> directionX, vec<T> directionY)
+		: plane<T>((directionX.cross(directionY)).norm(), center)
 	{	
-		C = center;
 		X = directionX;
 		Y = directionY;
 		scale(size[0], size[1]);
 	}
-	
-	CUDA_CALLABLE void scale(T factor1, T factor2){
+
+	CUDA_CALLABLE void scale(T factor){
+		X *= factor;
+		Y *= factor;
+	}
+
+	///scales a rectangle in ND space.
+	///@param factor1: size of the scale in the X-direction.
+	///@param factor2: size of the scale in the Y-direction.	
+	CUDA_CALLABLE void scale(T factor1, T factor2)
+	{
 		X *= factor1;
 		Y *= factor2;
 	}
+	///@param n; vector with the normal.
+	///Orients the rectangle along the normal n.
+	CUDA_CALLABLE void normal(vec<T> n)
+	{	
+		//orient the rectangle along the specified normal
+		rotate(n, X, Y);
+	}
+
+	///general init method that sets a general rectangle.
+	CUDA_CALLABLE void init()
+	{
+		X = vec<T>(1, 0, 0);
+		Y = vec<T>(0, 1, 0);
+	}
+
 	//boolean comparison
 	bool operator==(const rect<T> & rhs)
 	{
-		if(C == rhs.C && X == rhs.X && Y == rhs.Y)
+		if(P == rhs.P && X == rhs.X && Y == rhs.Y)
 			return true;
 		else
 			return false;
 	}
-	/*******************************************
-	Return the normal for the rect
-	*******************************************/
-	CUDA_CALLABLE stim::vec<T> n()
-	{
-        return (X.cross(Y)).norm();
-	}
 	//get the world space value given the planar coordinates a, b in [0, 1]
 	CUDA_CALLABLE stim::vec<T> p(T a, T b)
 	{
 		stim::vec<T> result;
 		//given the two parameters a, b = [0 1], returns the position in world space
-		vec<T> A = C - X * (T)0.5 - Y * (T)0.5;
+		vec<T> A = this->P - X * (T)0.5 - Y * (T)0.5;
 		result = A + X * a + Y * b;
 		return result;
@@ -142,16 +181,16 @@ public:
 	std::string str()
 	{
 		std::stringstream ss;
-		vec<T> A = C - X * (T)0.5 - Y * (T)0.5;
+		vec<T> A = P - X * (T)0.5 - Y * (T)0.5;
 		ss<<std::left<<"B="<<std::setfill('-')<<std::setw(20)<<A + Y<<">"<<"C="<<A + Y + X<<std::endl;
 		ss<<std::setfill(' ')<<std::setw(23)<<"|"<<"|"<<std::endl<<std::setw(23)<<"|"<<"|"<<std::endl;
 		ss<<std::left<<"A="<<std::setfill('-')<<std::setw(20)<<A<<">"<<"D="<<A + X;
-        return ss.str();
+        	return ss.str();
 	}
-	//scales the rectangle by a value rhs
+	///multiplication operator scales the rectangle by a value rhs.
 	CUDA_CALLABLE rect<T> operator*(T rhs)
 	{
 		//scales the plane by a scalar value
@@ -164,36 +203,44 @@ public:
 	}
-	//computes the distance between the specified point and this rectangle
+	///computes the distance between the specified point and this rectangle.
+	///@param p: x, y, z coordinates of the point to calculate distance to.
 	CUDA_CALLABLE T dist(vec<T> p)
 	{
         //compute the distance between a point and this rect
-		vec<T> A = C - X * (T)0.5 - Y * (T)0.5;
+		vec<T> A = P - X * (T)0.5 - Y * (T)0.5;
-        //first break the rect up into two triangles
-        triangle<T> T0(A, A+X, A+Y);
-        triangle<T> T1(A+X+Y, A+X, A+Y);
+		//first break the rect up into two triangles
+		triangle<T> T0(A, A+X, A+Y);
+		triangle<T> T1(A+X+Y, A+X, A+Y);
-        T d0 = T0.dist(p);
-        T d1 = T1.dist(p);
+		T d0 = T0.dist(p);
+		T d1 = T1.dist(p);
-        if(d0 < d1)
-            return d0;
-        else
-            return d1;
+		if(d0 < d1)
+		    return d0;
+		else
+		    return d1;
+	}
+
+	CUDA_CALLABLE T center(vec<T> p)
+	{
+		this->P = p;
 	}
+	///Returns the maximum distance of the rectangle from a point p to the sides of the rectangle.
+	///@param p: x, y, z point.
 	CUDA_CALLABLE T dist_max(vec<T> p)
 	{
-		vec<T> A = C - X * (T)0.5 - Y * (T)0.5;
-        T da = (A - p).len();
-        T db = (A+X - p).len();
-        T dc = (A+Y - p).len();
-        T dd = (A+X+Y - p).len();
+		vec<T> A = P - X * (T)0.5 - Y * (T)0.5;
+		T da = (A - p).len();
+		T db = (A+X - p).len();
+		T dc = (A+Y - p).len();
+		T dd = (A+X+Y - p).len();
-        return std::max( da, std::max(db, std::max(dc, dd) ) );
+		return std::max( da, std::max(db, std::max(dc, dd) ) );
 	}
 };