/****************************************************************** * * * Nest version 2 - 3D Wasp Nest Simulator * * Copyright (C) 1997 Sylvain GUERIN * * LIASC, ENST Bretagne & Santa Fe Institute * * * ****************************************************************** E-mail: Sylvain.Guerin@enst-bretagne.fr or: Sylvain Guerin 13 rue des Monts Lorgeaux, 51460 L'EPINE, FRANCE ****************************************************************** This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. ****************************************************************** Name : tools.c Component : 3D visualisation Fonctionality : contains methods for objects used in 3D visualisation ******************************************************************/ #include "tools.h" #include void swap_double (double& a, double& b) { double sw; sw = a; a = b; b = sw; } Facette::Facette (position anOrg, position p0, position p1, position p2, position p3) { point[0] = p0; point[1] = p1; point[2] = p2; point[3] = p3; nbPoints = 4; facettePlan = get_plan_from_3_positions (p0, p1, p2); org = anOrg; } Facette::Facette (position anOrg, position p0, position p1, position p2, position p3, position p4, position p5) { point[0] = p0; point[1] = p1; point[2] = p2; point[3] = p3; point[4] = p4; point[5] = p5; nbPoints = 6; facettePlan = get_plan_from_3_positions (p0, p1, p2); org = anOrg; } Facette::Facette () { } Facette::~Facette () { } void Facette::set (position anOrg, position p0, position p1, position p2, position p3) { point[0] = p0; point[1] = p1; point[2] = p2; point[3] = p3; nbPoints = 4; facettePlan = get_plan_from_3_positions (p0, p1, p2); org = anOrg; } void Facette::set (position anOrg, position p0, position p1, position p2, position p3, position p4, position p5) { point[0] = p0; point[1] = p1; point[2] = p2; point[3] = p3; point[4] = p4; point[5] = p5; nbPoints = 6; facettePlan = get_plan_from_3_positions (p0, p1, p2); org = anOrg; } direction Facette::getNormalDirection () { direction returnedDirection; returnedDirection.dx = facettePlan.a; returnedDirection.dy = facettePlan.b; returnedDirection.dz = facettePlan.c; return returnedDirection; } Vector Facette::getNormalVector () { direction foundDirection; position facettePoint; /* proj of org on Facette */ Vector returnedVector; /* USELESS int i; */ facettePoint = projection_on_plan (org, facettePlan); foundDirection = get_direction_from_2_positions (facettePoint, org); returnedVector.set (foundDirection.dx, foundDirection.dy, foundDirection.dz); return returnedVector; } // ---------------------------------------------------- // CLASS VECTOR // ---------------------------------------------------- // Constructors Vector::Vector () { #ifdef TRACE7 printf ("Default constructor for vector\n"); #endif } Vector::Vector (double x, double y, double z) { #ifdef TRACE7 printf ("Constructor for vector with values\n"); #endif v[0] = x; v[1] = y; v[2] = z; } Vector::~Vector () { #ifdef TRACE7 printf ("Default destructor for vector\n"); #endif } void Vector::set (double x, double y, double z) { v[0] = x; v[1] = y; v[2] = z; } void Vector::display () { printf (" [ %9.6f ]\n", v[0]); printf (" [ %9.6f ]\n", v[1]); printf (" [ %9.6f ]\n", v[2]); } double Vector::x () { return v[0]; } double Vector::y () { return v[1]; } double Vector::z () { return v[2]; } void Vector::scal (double k) { #ifdef TRACE7 printf ("Vector scal with %f\n", k); #endif v[0] = k*v[0]; v[1] = k*v[1]; v[2] = k*v[2]; } Vector operator+ (Vector& v1, Vector& v2) { #ifdef TRACE7 printf ("Addition two vectors\n"); #endif return Vector (v1.v[0]+v2.v[0], v1.v[1]+v2.v[1], v1.v[2]+v2.v[2]); } Vector operator- (Vector& v1, Vector& v2) { #ifdef TRACE7 printf ("Substraction two vectors\n"); #endif return Vector (v1.v[0]-v2.v[0], v1.v[1]-v2.v[1], v1.v[2]-v2.v[2]); } Vector operator* (double k, Vector& v) { #ifdef TRACE7 printf ("Vector scal with %f\n", k); #endif return Vector (k*v.v[0], k*v.v[1], k*v.v[2]); } // ---------------------------------------------------- // CLASS MATRIX // ---------------------------------------------------- // Constructors Matrix::Matrix () { #ifdef TRACE7 printf ("Default constructor for matrix\n"); #endif } Matrix::Matrix (double xx, double xy, double xz, double yx, double yy, double yz, double zx, double zy, double zz) { #ifdef TRACE7 printf ("Constructor for matrix with values\n"); #endif m[0][0] = xx; m[0][1] = xy; m[0][2] = xz; m[1][0] = yx; m[1][1] = yy; m[1][2] = yz; m[2][0] = zx; m[2][1] = zy; m[2][2] = zz; } double Matrix::xx () { return m[0][0]; } double Matrix::xy () { return m[0][1]; } double Matrix::xz () { return m[0][2]; } double Matrix::yx () { return m[1][0]; } double Matrix::yy () { return m[1][1]; } double Matrix::yz () { return m[1][2]; } double Matrix::zx () { return m[2][0]; } double Matrix::zy () { return m[2][1]; } double Matrix::zz () { return m[2][2]; } Matrix::~Matrix () { #ifdef TRACE7 printf ("Default destructor for matrix\n"); #endif } void Matrix::display () { printf (" [ %15.15f %15.15f %15.15f ]\n", m[0][0], m[0][1], m[0][2]); printf (" [ %15.15f %15.15f %15.15f ]\n", m[1][0], m[1][1], m[1][2]); printf (" [ %15.15f %15.15f %15.15f ]\n", m[2][0], m[2][1], m[2][2]); } void Matrix::scal (double k) { #ifdef TRACE7 printf ("Matrix scal with %f\n", k); #endif m[0][0] = k*m[0][0]; m[0][1] = k*m[0][1]; m[0][2] = k*m[0][2]; m[1][0] = k*m[1][0]; m[1][1] = k*m[1][1]; m[1][2] = k*m[1][2]; m[2][0] = k*m[2][0]; m[2][1] = k*m[2][1]; m[2][2] = k*m[2][2]; } void Matrix::tr () { #ifdef TRACE7 printf ("transposate following Matrix\n"); display (); #endif swap_double (m[0][1], m[1][0]); swap_double (m[0][2], m[2][0]); swap_double (m[1][2], m[2][1]); } int Matrix::inverse () { #ifdef TRACE7 printf ("inverse following Matrix\n"); display (); #endif Matrix temp; double determinant; temp = *this; determinant = det(*this); if (determinant !=0) { m[0][0] = det2x2 (temp.yy (), temp.yz (), temp.zy (), temp.zz ()); m[1][0] = -det2x2 (temp.xy (), temp.xz (), temp.zy (), temp.zz ()); m[2][0] = det2x2 (temp.xy (), temp.xz (), temp.yy (), temp.yz ()); m[0][1] = -det2x2 (temp.yx (), temp.yz (), temp.zx (), temp.zz ()); m[1][1] = det2x2 (temp.xx (), temp.xz (), temp.zx (), temp.zz ()); m[2][1] = -det2x2 (temp.xx (), temp.xz (), temp.yx (), temp.yz ()); m[0][2] = det2x2 (temp.yx (), temp.yy (), temp.zx (), temp.zy ()); m[1][2] = -det2x2 (temp.xx (), temp.xy (), temp.zx (), temp.zy ()); m[2][2] = det2x2 (temp.xx (), temp.xy (), temp.yx (), temp.yy ()); tr (); scal (1/determinant); return 1; } else { scal (0); return 0; } } void Matrix::set_rotation (double teta, double phi) { #ifdef TRACE7 printf ("set_rotation with teta=%9.6f phi=%9.6f\n", teta, phi); #endif Matrix m1, m2, m3; m1 = Matrix (1,0,0,0,-cos(phi),-sin(phi),0,sin(phi),-cos(phi)); m2 = Matrix (sin(teta), 0, -cos(teta),0,1,0,cos(teta),0,sin(teta)); m3 = m1*m2; *this = m3; } /* void Matrix::set_matrix (Matrix& m) { #ifdef TRACE7 printf ("set_matrix\n"); #endif m[0][0] = m.xx (); m[0][1] = m.xy (); m[0][2] = m.xz (); m[1][0] = m.yx (); m[1][1] = m.yy (); m[1][2] = m.yz (); m[2][0] = m.zx (); m[2][1] = m.zy (); m[2][2] = m.zz (); } */ double det2x2 (double a, double c, double b, double d) { //#ifdef TRACE7 //printf ("calculate det 2x2 (%9.6f %9.6f %9.6f %9.6f)\n", a, c, b, d); //#endif return (a*d-b*c); } double det (Matrix& m) { #ifdef TRACE7 printf ("calculate det for Matrix\n"); #endif double det; det = m.m[0][0]*det2x2 (m.m[1][1], m.m[1][2], m.m[2][1], m.m[2][2]) - m.m[1][0]*det2x2 (m.m[0][1], m.m[0][2], m.m[2][1], m.m[2][2]) + m.m[2][0]*det2x2 (m.m[0][1], m.m[0][2], m.m[1][1], m.m[1][2]); #ifdef TRACE7 printf ("Result: %f\n", det); #endif return det; } Matrix operator+ (Matrix& m1, Matrix& m2) { #ifdef TRACE7 printf ("Addition two Matrix\n"); #endif return Matrix (m1.m[0][0]+m2.m[0][0], m1.m[0][1]+m2.m[0][1], m1.m[0][2]+m2.m[0][2], m1.m[1][0]+m2.m[1][0], m1.m[1][1]+m2.m[1][1], m1.m[1][2]+m2.m[1][2], m1.m[2][0]+m2.m[2][0], m1.m[2][1]+m2.m[2][1], m1.m[2][2]+m2.m[2][2]); } Matrix operator- (Matrix& m1, Matrix& m2) { #ifdef TRACE7 printf ("Substraction two Matrix\n"); #endif return Matrix (m1.m[0][0]-m2.m[0][0], m1.m[0][1]-m2.m[0][1], m1.m[0][2]-m2.m[0][2], m1.m[1][0]-m2.m[1][0], m1.m[1][1]-m2.m[1][1], m1.m[1][2]-m2.m[1][2], m1.m[2][0]-m2.m[2][0], m1.m[2][1]-m2.m[2][1], m1.m[2][2]-m2.m[2][2]); } Matrix operator* (Matrix& m1, Matrix& m2) { #ifdef TRACE7 printf ("Multiplication two Matrix\n"); #endif return Matrix (mult_lin_col (m1, 0, m2, 0), mult_lin_col (m1, 0, m2, 1), mult_lin_col (m1, 0, m2, 2), mult_lin_col (m1, 1, m2, 0), mult_lin_col (m1, 1, m2, 1), mult_lin_col (m1, 1, m2, 2), mult_lin_col (m1, 2, m2, 0), mult_lin_col (m1, 2, m2, 1), mult_lin_col (m1, 2, m2, 2)); } Matrix operator* (double k, Matrix& m) { #ifdef TRACE7 printf ("Matrix scal with %f\n", k); #endif return Matrix (k*m.m[0][0], k*m.m[0][1], k*m.m[0][2], k*m.m[1][0], k*m.m[1][1], k*m.m[1][2], k*m.m[2][0], k*m.m[2][1], k*m.m[2][2]); } Matrix tr (Matrix& m) { #ifdef TRACE7 printf ("transposate Matrix\n"); #endif return Matrix (m.m[0][0], m.m[1][0], m.m[2][0], m.m[0][1], m.m[1][1], m.m[2][1], m.m[0][2], m.m[1][2], m.m[2][2]); } Matrix inverse (Matrix& m) { Matrix inversed_matrix; inversed_matrix = m; inversed_matrix.inverse (); return inversed_matrix; } Vector operator* (Matrix& m, Vector& v) { #ifdef TRACE7 printf ("calculate Matrix x Vector\n"); #endif return Vector (m.m[0][0]*v.v[0]+m.m[0][1]*v.v[1]+m.m[0][2]*v.v[2], m.m[1][0]*v.v[0]+m.m[1][1]*v.v[1]+m.m[1][2]*v.v[2], m.m[2][0]*v.v[0]+m.m[2][1]*v.v[1]+m.m[2][2]*v.v[2]); } double mult_lin_col (Matrix& m1, int lin1, Matrix& m2, int col2) { return (m1.m[lin1][0]*m2.m[0][col2] + m1.m[lin1][1]*m2.m[1][col2] + m1.m[lin1][2]*m2.m[2][col2]); } // ---------------------------------------------------- // CLASS VECTOR2 // ---------------------------------------------------- // Constructors Vector2::Vector2 () { #ifdef TRACE7 printf ("Default constructor for vector2\n"); #endif } Vector2::Vector2 (double x, double y) { #ifdef TRACE7 printf ("Constructor for vector2 with values\n"); #endif v[0] = x; v[1] = y; } Vector2::~Vector2 () { #ifdef TRACE7 printf ("Default destructor for vector2\n"); #endif } void Vector2::display () { printf (" [ %9.6f ]\n", v[0]); printf (" [ %9.6f ]\n", v[1]); } double Vector2::x () { return v[0]; } double Vector2::y () { return v[1]; } void Vector2::scal (double k) { #ifdef TRACE7 printf ("Vector2 scal with %f\n", k); #endif v[0] = k*v[0]; v[1] = k*v[1]; } Vector2 operator+ (Vector2& v1, Vector2& v2) { #ifdef TRACE7 printf ("Addition two vector2s\n"); #endif return Vector2 (v1.v[0]+v2.v[0], v1.v[1]+v2.v[1]); } Vector2 operator- (Vector2& v1, Vector2& v2) { #ifdef TRACE7 printf ("Substraction two vector2s\n"); #endif return Vector2 (v1.v[0]-v2.v[0], v1.v[1]-v2.v[1]); } Vector2 operator* (double k, Vector2& v) { #ifdef TRACE7 printf ("Vector2 scal with %f\n", k); #endif return Vector2 (k*v.v[0], k*v.v[1]); } // ---------------------------------------------------- // CLASS MATRIX2 // ---------------------------------------------------- // Constructors Matrix2::Matrix2 () { #ifdef TRACE7 printf ("Default constructor for matrix2\n"); #endif } Matrix2::Matrix2 (double xx, double xy, double yx, double yy) { #ifdef TRACE7 printf ("Constructor for matrix2 with values\n"); #endif m[0][0] = xx; m[0][1] = xy; m[1][0] = yx; m[1][1] = yy; } double Matrix2::xx () { return m[0][0]; } double Matrix2::xy () { return m[0][1]; } double Matrix2::yx () { return m[1][0]; } double Matrix2::yy () { return m[1][1]; } Matrix2::~Matrix2 () { #ifdef TRACE7 printf ("Default destructor for matrix2\n"); #endif } void Matrix2::display () { printf (" [ %9.6f %9.6f ]\n", m[0][0], m[0][1]); printf (" [ %9.6f %9.6f ]\n", m[1][0], m[1][1]); } void Matrix2::scal (double k) { #ifdef TRACE7 printf ("Matrix2 scal with %f\n", k); #endif m[0][0] = k*m[0][0]; m[0][1] = k*m[0][1]; m[1][0] = k*m[1][0]; m[1][1] = k*m[1][1]; } void Matrix2::tr () { #ifdef TRACE7 printf ("transposate following Matrix2\n"); display (); #endif swap_double (m[0][1], m[1][0]); } int Matrix2::inverse () { #ifdef TRACE7 printf ("inverse following Matrix2\n"); display (); #endif Matrix2 temp; double determinant; temp = *this; determinant = det(*this); if (determinant !=0) { m[0][0] = temp.yy (); m[1][0] = -temp.xy (); m[0][1] = -temp.yx (); m[1][1] = temp.xx (); tr (); scal (1/determinant); return 1; } else { scal (0); return 0; } } double det (Matrix2& m) { #ifdef TRACE7 printf ("calculate det for Matrix2\n"); #endif double det; det = det2x2 (m.m[0][0], m.m[0][1], m.m[1][0], m.m[1][1]); #ifdef TRACE7 printf ("Result: %f\n", det); #endif return det; } Matrix2 operator+ (Matrix2& m1, Matrix2& m2) { #ifdef TRACE7 printf ("Addition two Matrix2\n"); #endif return Matrix2 (m1.m[0][0]+m2.m[0][0], m1.m[0][1]+m2.m[0][1], m1.m[1][0]+m2.m[1][0], m1.m[1][1]+m2.m[1][1]); } Matrix2 operator- (Matrix2& m1, Matrix2& m2) { #ifdef TRACE7 printf ("Substraction two Matrix2\n"); #endif return Matrix2 (m1.m[0][0]-m2.m[0][0], m1.m[0][1]-m2.m[0][1], m1.m[1][0]-m2.m[1][0], m1.m[1][1]-m2.m[1][1]); } Matrix2 operator* (Matrix2& m1, Matrix2& m2) { #ifdef TRACE7 printf ("Multiplication two Matrix2\n"); #endif return Matrix2 (mult_lin_col_2x2 (m1, 0, m2, 0), mult_lin_col_2x2 (m1, 0, m2, 1), mult_lin_col_2x2 (m1, 1, m2, 0), mult_lin_col_2x2 (m1, 1, m2, 1)); } Matrix2 operator* (double k, Matrix2& m) { #ifdef TRACE7 printf ("Matrix2 scal with %f\n", k); #endif return Matrix2 (k*m.m[0][0], k*m.m[0][1], k*m.m[1][0], k*m.m[1][1]); } Matrix2 tr (Matrix2& m) { #ifdef TRACE7 printf ("transposate Matrix2\n"); #endif return Matrix2 (m.m[0][0], m.m[1][0], m.m[0][1], m.m[1][1]); } Matrix2 inverse (Matrix2& m) { Matrix2 inversed_matrix; inversed_matrix = m; inversed_matrix.inverse (); return inversed_matrix; } Vector2 operator* (Matrix2& m, Vector2& v) { #ifdef TRACE7 printf ("calculate Matrix2 x Vector2\n"); #endif return Vector2 (m.m[0][0]*v.v[0]+m.m[0][1]*v.v[1], m.m[1][0]*v.v[0]+m.m[1][1]*v.v[1]); } double mult_lin_col_2x2 (Matrix2& m1, int lin1, Matrix2& m2, int col2) { return (m1.m[lin1][0]*m2.m[0][col2] + m1.m[lin1][1]*m2.m[1][col2]); } direction get_direction_from_2_positions (position p1, position p2) { direction ret_dir; double k; ret_dir.dx = p2.x - p1.x; ret_dir.dy = p2.y - p1.y; ret_dir.dz = p2.z - p1.z; k = sqrt (ret_dir.dx*ret_dir.dx+ret_dir.dy*ret_dir.dy+ret_dir.dz*ret_dir.dz); ret_dir.dx = ret_dir.dx/k; ret_dir.dy = ret_dir.dy/k; ret_dir.dz = ret_dir.dz/k; return ret_dir; } plan get_plan_from_direction_and_position (direction d, position p) { plan ret_plan; double k; ret_plan.a = d.dx; ret_plan.b = d.dy; ret_plan.c = d.dz; ret_plan.d = ret_plan.a*p.x + ret_plan.b*p.y + ret_plan.c*p.z; k = sqrt (ret_plan.a*ret_plan.a+ret_plan.b*ret_plan.b+ret_plan.c*ret_plan.c); if ((ret_plan.a<0) || ((ret_plan.a==0) && (ret_plan.b<0)) || ((ret_plan.a==0) && (ret_plan.b==0) && (ret_plan.c<0))) k = -k; ret_plan.a = ret_plan.a/k; ret_plan.b = ret_plan.b/k; ret_plan.c = ret_plan.c/k; ret_plan.d = ret_plan.d/k; #ifdef TRACE6 printf ("returned_plan = (%9.3f %9.3f %9.3f %9.3f)\n", ret_plan.a, ret_plan.b, ret_plan.c, ret_plan.d); #endif return ret_plan; } void get_teta_phi_from_direction (direction viewed_direction, double* p_teta, double* p_phi) { double teta, phi; phi = asin (viewed_direction.dy); // may be phi = M_PI-phi or phi = -M_PI-phi if (!equal(cos (phi),0)) { // phi != M_PI/2 and phi != -M_PI/2 teta = acos (viewed_direction.dx/cos(phi)); // may be teta = -teta; if (cos(phi)*cos(teta)*viewed_direction.dx < 0) if (phi>0) phi = M_PI-phi; else phi = -M_PI-phi; if (cos(phi)*sin(teta)*viewed_direction.dz < 0) { teta = -teta; } phi = -phi; if (phi>M_PI/2) { phi = M_PI-phi; if (teta>0) teta = teta-M_PI; else teta = teta+M_PI; } else if (phi<-M_PI/2) { phi = -M_PI-phi; if (teta>0) teta = teta-M_PI; else teta = teta+M_PI; } *p_teta = teta; *p_phi = phi; } else { /* values aren't modified */ } } double get_dist_positions (position p1, position p2) { return sqrt((p1.x-p2.x)*(p1.x-p2.x) +(p1.y-p2.y)*(p1.y-p2.y) +(p1.z-p2.z)*(p1.z-p2.z)); } position projection_on_plan (position pos, plan proj_plan) { double a, k; position ret; a = proj_plan.a*proj_plan.a +proj_plan.b*proj_plan.b +proj_plan.c*proj_plan.c; if (a < epsilon) printf ("ERROR: definition of a plan is wrong. Unable to recover.\n"); else { k = (proj_plan.d-proj_plan.a*pos.x-proj_plan.b*pos.y-proj_plan.c*pos.z)/a; ret.x = k*proj_plan.a+pos.x; ret.y = k*proj_plan.b+pos.y; ret.z = k*proj_plan.c+pos.z; } return ret; } double maxim (double a, double b) { if (ay) { if (x-y>epsilon) return FALSE; else return TRUE; } else if (xepsilon) return FALSE; else return TRUE; } else return TRUE; } float cosDeg(int angle) { return (float)cos((double)(2*M_PI*(float)angle/360)); } float sinDeg(int angle) { return (float)sin((double)(2*M_PI*(float)angle/360)); } position init_position () { position returned; returned.x = 0; returned.y = 0; returned.z = 0; return returned; } position_plan double_to_int (position_plan_double input) { position_plan ret; ret.x = (int)input.x; ret.y = (int)input.y; return ret; } plan get_plan_from_3_positions (position p1, position p2, position p3) { plan ret_plan; Matrix syst_matrix; Vector result; Vector i3(1,1,1); t_boolean res; double determ; double k; syst_matrix = Matrix (p1.x, p1.y, p1.z, p2.x, p2.y, p2.z, p3.x, p3.y, p3.z); determ = det (syst_matrix); if (determ != 0) { /* origine doesn't belong to plan */ syst_matrix = inverse (syst_matrix); result = syst_matrix*i3; ret_plan.a = result.x (); ret_plan.b = result.y (); ret_plan.c = result.z (); k = sqrt (ret_plan.a*ret_plan.a+ret_plan.b*ret_plan.b+ret_plan.c*ret_plan.c); if ((ret_plan.a<0) || ((ret_plan.a==0) && (ret_plan.b<0)) || ((ret_plan.a==0) && (ret_plan.b==0) && (ret_plan.c<0))) k = -k; ret_plan.d = 1/k; ret_plan.a = ret_plan.a/k; ret_plan.b = ret_plan.b/k; ret_plan.c = ret_plan.c/k; } else { /* origine belongs to plan */ Matrix2 reduced_matrix(0,0,0,0); Matrix2 tested_matrix; Vector2 vector; int i, j; int io, jo; position pt1, pt2; position pf1, pf2; ret_plan.d = 0; jo = -1 ; /* needed by g++ -O2 */ for (i=0; i<3; i++) { switch (i) { case 0: /* line 0 */ pt1=p2; pt2=p3; break; case 1: /* line 1 */ pt1=p1; pt2=p3; break; case 2: /* line 2 */ pt1=p1; pt2=p2; break; } for (j=0; j<3; j++) { switch (j) { case 0: tested_matrix = Matrix2 (pt1.y, pt1.z, pt2.y, pt2.z); break; case 1: tested_matrix = Matrix2 (pt1.x, pt1.z, pt2.x, pt2.z); break; case 2: tested_matrix = Matrix2 (pt1.x, pt1.y, pt2.x, pt2.y); break; } if (det (tested_matrix) != 0) { io = i; jo = j; pf1 = pt1; pf2 = pt2; reduced_matrix = tested_matrix; } } } /* We've got an inversable matrix*/ if (det (reduced_matrix) == 0) printf ("ERROR: request for calculate plan with 3 points on same line\n"); reduced_matrix = inverse (reduced_matrix); switch (jo) { case 0: vector = Vector2 (-pf1.x, -pf2.x); ret_plan.a = 1; vector = reduced_matrix*vector; ret_plan.b = vector.x (); ret_plan.c = vector.y (); break; case 1: vector = Vector2 (-pf1.y, -pf2.y); ret_plan.b = 1; vector = reduced_matrix*vector; ret_plan.a = vector.x (); ret_plan.c = vector.y (); break; case 2: vector = Vector2 (-pf1.z, -pf2.z); ret_plan.c = 1; vector = reduced_matrix*vector; ret_plan.a = vector.x (); ret_plan.b = vector.y (); break; } k = sqrt (ret_plan.a*ret_plan.a+ret_plan.b*ret_plan.b+ret_plan.c*ret_plan.c); if ((ret_plan.a<0) || ((ret_plan.a==0) && (ret_plan.b<0)) || ((ret_plan.a==0) && (ret_plan.b==0) && (ret_plan.c<0))) k = -k; ret_plan.a = ret_plan.a/k; ret_plan.b = ret_plan.b/k; ret_plan.c = ret_plan.c/k; } res = belong_to_plan (p1, ret_plan); res = belong_to_plan (p2, ret_plan); res = belong_to_plan (p3, ret_plan); return ret_plan; } t_boolean belong_to_plan (position pos, plan p) { if (equal (pos.x*p.a+pos.y*p.b+pos.z*p.c, p.d)) { return TRUE; } else { return FALSE; } } position setPosition (double ix, double iy, double iz) { position ret; ret.x = ix; ret.y = iy; ret.z = iz; return ret; }