1 /* Point3D.c */
2 /**********************************************************************************************************
3 Copyright (c) 2002-2013 Abdul-Rahman Allouche. All rights reserved
4
5 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
6 documentation files (the Gabedit), to deal in the Software without restriction, including without limitation
7 the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software,
8 and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
9
10 The above copyright notice and this permission notice shall be included in all copies or substantial portions
11 of the Software.
12
13 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
14 TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
15 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
16 CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
17 DEALINGS IN THE SOFTWARE.
18 ************************************************************************************************************/
19
20
21 #include "../../Config.h"
22 #include <math.h>
23 #include "../Common/Global.h"
24 #include "../Utils/Point3D.h"
25
26 /************************************************************************************************************/
27 static gdouble length(Point3D* p);
28 static gboolean unit(Point3D* p);
29 static Point3D add(Point3D* p, Point3D* other);
30 static Point3D sub(Point3D* p, Point3D* other);
31 static Point3D mult(Point3D* p, gdouble factor);
32 static Point3D crossProd(Point3D* p, Point3D* other);
33 static gdouble dotProd(Point3D* p, Point3D* other);
34 static gdouble distance(Point3D* p, Point3D* other);
35 static gdouble angleBetween(Point3D* p, Point3D* other);
36 static Point3D rotate(Point3D* p, Point3D* rotCenter, Point3D* rotateAxis, gdouble angle);
37 static gboolean equals(Point3D* p, Point3D* other);
38 static Point3D closestPointInPlane(Point3D* point, Point3D* planePoint, Point3D* normal);
39 static Point3D closestPointOnAxis(Point3D* point, Point3D* axisPoint, Point3D* paxis);
40 static gchar* toString(Point3D* p);
41 static Point3D* copy(Point3D* p);
42 static Point3D* copyPoint(Point3D p);
43 /************************************************************************************************************/
newPoint3D()44 Point3D newPoint3D()
45 {
46 Point3D p;
47 p.x = 0;
48 p.y = 0;
49 p.z = 0;
50 sprintf(p.str," ");
51 p.length = length;
52 p.unit = unit;
53 p.add = add;
54 p.sub = sub;
55 p.mult = mult;
56 p.copy = copy;
57 p.copyPoint = copyPoint;
58 p.crossProd = crossProd;
59 p.dotProd = dotProd;
60 p.angleBetween = angleBetween;
61 p.rotate = rotate;
62 p.equals = equals;
63 p.closestPointInPlane = closestPointInPlane;
64 p.closestPointOnAxis = closestPointOnAxis;
65 p.distance = distance;
66 p.toString = toString;
67 return p;
68 };
69 /************************************************************************************************************/
newPoint3Dxyz(gdouble x,gdouble y,gdouble z)70 Point3D newPoint3Dxyz(gdouble x, gdouble y, gdouble z)
71 {
72 Point3D p = newPoint3D();
73 p.x = x;
74 p.y = y;
75 p.z = z;
76 return p;
77 }
78 /************************************************************************************************************/
length(Point3D * p)79 static gdouble length(Point3D* p)
80 {
81 return sqrt(p->x*p->x + p->y*p->y + p->z*p->z);
82 }
83 /************************************************************************************************************/
unit(Point3D * p)84 static gboolean unit(Point3D* p)
85 {
86 gdouble len = length(p);
87 if(len != 0)
88 {
89 p->x /= len;
90 p->y /= len;
91 p->z /= len;
92 return TRUE;
93 }
94 return FALSE;
95 }
96 /************************************************************************************************************/
add(Point3D * p,Point3D * other)97 static Point3D add(Point3D* p, Point3D* other)
98 {
99 return newPoint3Dxyz(p->x + other->x, p->y + other->y, p->z + other->z);
100 }
101 /************************************************************************************************************/
sub(Point3D * p,Point3D * other)102 static Point3D sub(Point3D* p, Point3D* other)
103 {
104 return newPoint3Dxyz(p->x - other->x, p->y - other->y, p->z - other->z);
105 }
106 /************************************************************************************************************/
mult(Point3D * p,gdouble factor)107 static Point3D mult(Point3D* p, gdouble factor)
108 {
109 return newPoint3Dxyz(p->x *factor, p->y * factor, p->z * factor);
110 }
111 /************************************************************************************************************/
crossProd(Point3D * p,Point3D * other)112 static Point3D crossProd(Point3D* p, Point3D* other)
113 {
114 Point3D result = newPoint3D();
115 result.x = other->z*p->y - other->y*p->z;
116 result.y = other->x*p->z - other->z*p->x;
117 result.z = other->y*p->x - other->x*p->y;
118 return result;
119 }
120 /************************************************************************************************************/
copy(Point3D * p)121 static Point3D* copy(Point3D* p)
122 {
123 Point3D* newP = g_malloc(sizeof(Point3D));
124 *newP = *p;
125 return newP;
126 }
127 /************************************************************************************************************/
copyPoint(Point3D p)128 static Point3D* copyPoint(Point3D p)
129 {
130 return copy(&p);
131 }
132 /************************************************************************************************************/
dotProd(Point3D * p,Point3D * other)133 static gdouble dotProd(Point3D* p, Point3D* other)
134 {
135 return other->x*p->x + other->y*p->y + other->z*p->z;
136 }
137 /************************************************************************************************************/
distance(Point3D * p,Point3D * other)138 static gdouble distance(Point3D* p, Point3D* other)
139 {
140 gdouble dx = other->x-p->x;
141 gdouble dy = other->y-p->y;
142 gdouble dz = other->z-p->z;
143 return sqrt(dx*dx+dy*dy+dz*dz);
144 }
145 /************************************************************************************************************/
angleBetween(Point3D * p,Point3D * other)146 static gdouble angleBetween(Point3D* p, Point3D* other)
147 {
148 return acos(dotProd(p,other)/(length(p)*length(other)));
149 }
150 /************************************************************************************************************/
rotate(Point3D * p,Point3D * rotCenter,Point3D * rotateAxis,gdouble angle)151 static Point3D rotate(Point3D* p, Point3D* rotCenter, Point3D* rotateAxis, gdouble angle)
152 {
153 Point3D oldCoords = *p;
154 Point3D newCoords = newPoint3D();
155 Point3D rotAxis = *rotateAxis;
156 unit(&rotAxis);
157
158 oldCoords = sub(&oldCoords, rotCenter);
159
160 gdouble c = cos(angle);
161 gdouble s = sin(angle);
162
163 gdouble m[4][4];
164
165 m[0][0] = c + (1 - c) * rotAxis.x * rotAxis.x;
166 m[0][1] = (1 - c) * rotAxis.x * rotAxis.y - s * rotAxis.z;
167 m[0][2] = (1 - c) * rotAxis.x * rotAxis.z + s * rotAxis.y;
168 m[1][0] = (1 - c) * rotAxis.x * rotAxis.y + s * rotAxis.z;
169 m[1][1] = c + (1 - c) * rotAxis.y * rotAxis.y;
170 m[1][2] = (1 - c) * rotAxis.z * rotAxis.y - s * rotAxis.x;
171 m[2][0] = (1 - c) * rotAxis.x * rotAxis.z - s * rotAxis.y;
172 m[2][1] = (1 - c) * rotAxis.y * rotAxis.z + s * rotAxis.x;
173 m[2][2] = c + (1 - c) * rotAxis.z * rotAxis.z;
174
175 newCoords.x = m[0][0]*oldCoords.x + m[0][1]*oldCoords.y + m[0][2]*oldCoords.z;
176 newCoords.y = m[1][0]*oldCoords.x + m[1][1]*oldCoords.y + m[1][2]*oldCoords.z;
177 newCoords.z = m[2][0]*oldCoords.x + m[2][1]*oldCoords.y + m[2][2]*oldCoords.z;
178
179 newCoords = add(&newCoords, rotCenter);
180
181 return newCoords;
182 }
183 /************************************************************************************************************/
equals(Point3D * p,Point3D * other)184 static gboolean equals(Point3D* p, Point3D* other)
185 {
186 if( !p || !other) return FALSE;
187 if(distance(p, other) < 0.00001) return TRUE;
188 return FALSE;
189 }
190 /************************************************************************************************************/
closestPointInPlane(Point3D * point,Point3D * planePoint,Point3D * normal)191 static Point3D closestPointInPlane(Point3D* point, Point3D* planePoint, Point3D* normal)
192 {
193 gdouble d = -(normal->x * planePoint->x + normal->y * planePoint->y + normal->z * planePoint->z);
194 gdouble t = -((normal->x * point->x + normal->y * point->y + normal->z * point->z + d)/
195 (normal->x*normal->x + normal->y*normal->y + normal->z*normal->z));
196 return newPoint3Dxyz(point->x + normal->x * t, point->y + normal->y * t, point->z + normal->z*t);
197 }
198 /************************************************************************************************************/
closestPointOnAxis(Point3D * point,Point3D * axisPoint,Point3D * paxis)199 static Point3D closestPointOnAxis(Point3D* point, Point3D* axisPoint, Point3D* paxis)
200 {
201 Point3D axis = newPoint3Dxyz(paxis->x, paxis->y, paxis->z);
202 unit(&axis);
203 Point3D s = sub(point,axisPoint);
204 Point3D m = mult(&axis, dotProd(&s,&axis));
205 return add(axisPoint, &m);
206 }
207 /************************************************************************************************************/
toString(Point3D * point)208 static gchar* toString(Point3D* point)
209 {
210 sprintf(point->str,"(%f, %f, %f)", point->x, point->y, point->z);
211 return point->str;
212 }
213