1 | // This file is a part of the Framsticks GDK. |
---|
2 | // Copyright (C) 2002-2014 Maciej Komosinski and Szymon Ulatowski. See LICENSE.txt for details. |
---|
3 | // Refer to http://www.framsticks.com/ for further information. |
---|
4 | |
---|
5 | #include <common/nonstd_math.h> |
---|
6 | #include <common/framsg.h> |
---|
7 | #include "3d.h" |
---|
8 | |
---|
9 | Pt3D operator+(const Pt3D &p1,const Pt3D &p2) {return Pt3D(p1.x+p2.x,p1.y+p2.y,p1.z+p2.z);} |
---|
10 | Pt3D operator-(const Pt3D &p1,const Pt3D &p2) {return Pt3D(p1.x-p2.x,p1.y-p2.y,p1.z-p2.z);} |
---|
11 | |
---|
12 | Pt3D Pt3D_0(0,0,0); |
---|
13 | |
---|
14 | bool Pt3D::report_errors=true; |
---|
15 | |
---|
16 | double Pt3D::operator()() const |
---|
17 | { |
---|
18 | double q=x*x+y*y+z*z; |
---|
19 | if (q<0) {if (report_errors) FramMessage("Pt3D","operator()","sqrt domain error",3); return 0;} |
---|
20 | return sqrt(q); |
---|
21 | } |
---|
22 | |
---|
23 | bool Pt3D::normalize() |
---|
24 | { |
---|
25 | double len=length(); |
---|
26 | if (fabs(len)<1e-50) {if (report_errors) FramMessage("Pt3D","normalize()","vector too small",1); x=1;y=0;z=0; return false;} |
---|
27 | operator/=(len); |
---|
28 | return true; |
---|
29 | } |
---|
30 | |
---|
31 | double Pt3D::distanceTo(const Pt3D& p) const |
---|
32 | { |
---|
33 | return sqrt((x-p.x)*(x-p.x)+(y-p.y)*(y-p.y)+(z-p.z)*(z-p.z)); |
---|
34 | } |
---|
35 | |
---|
36 | double Pt3D::manhattanDistanceTo(const Pt3D& p) const |
---|
37 | { |
---|
38 | return fabs(x-p.x)+fabs(y-p.y)+fabs(z-p.z); |
---|
39 | } |
---|
40 | |
---|
41 | Orient Orient_1(Pt3D(1,0,0),Pt3D(0,1,0),Pt3D(0,0,1)); |
---|
42 | |
---|
43 | // prosty obrot |
---|
44 | void rotate2D(double k,double &x,double &y) |
---|
45 | {double s=sin(k),c=cos(k); |
---|
46 | double t=c*x-s*y; y=s*x+c*y; x=t;} |
---|
47 | |
---|
48 | void rotate2D(double s,double c,double &x,double &y) |
---|
49 | {double t=c*x-s*y; y=s*x+c*y; x=t;} |
---|
50 | |
---|
51 | int Pt3D::getAngle(double dx,double dy,double &wyn) |
---|
52 | { |
---|
53 | if ((fabs(dx)+fabs(dy))<0.0001) return 0; |
---|
54 | wyn=atan2(dy,dx); |
---|
55 | return 1; |
---|
56 | } |
---|
57 | |
---|
58 | void Pt3D::getAngles(const Pt3D& X,const Pt3D& dir) |
---|
59 | { |
---|
60 | Pt3D t1(X), t2(dir); |
---|
61 | if (getAngle(t1.x,t1.y,z)) |
---|
62 | { // nie pionowo |
---|
63 | rotate2D(-z,t1.x,t1.y); |
---|
64 | rotate2D(-z,t2.x,t2.y); |
---|
65 | getAngle(t1.x, t1.z, y); |
---|
66 | } |
---|
67 | else |
---|
68 | { // pionowo |
---|
69 | z=0; |
---|
70 | if (t1.z<0) |
---|
71 | y=-M_PI_2; // w dol |
---|
72 | else |
---|
73 | y=M_PI_2; // w gore |
---|
74 | } |
---|
75 | rotate2D(-y,t2.x,t2.z); |
---|
76 | if (!getAngle(t2.z,-t2.y,x)) |
---|
77 | x=0;// to zly wynik ale dobrego nie ma :P |
---|
78 | } |
---|
79 | |
---|
80 | void Pt3D::getMin(const Pt3D& p) |
---|
81 | { |
---|
82 | if (p.x<x) x=p.x; |
---|
83 | if (p.y<y) y=p.y; |
---|
84 | if (p.z<z) z=p.z; |
---|
85 | } |
---|
86 | void Pt3D::getMax(const Pt3D& p) |
---|
87 | { |
---|
88 | if (p.x>x) x=p.x; |
---|
89 | if (p.y>y) y=p.y; |
---|
90 | if (p.z>z) z=p.z; |
---|
91 | } |
---|
92 | |
---|
93 | void Pt3D::vectorProduct(const Pt3D& a,const Pt3D& b) |
---|
94 | { |
---|
95 | x=a.y*b.z-a.z*b.y; |
---|
96 | y=a.z*b.x-a.x*b.z; |
---|
97 | z=a.x*b.y-a.y*b.x; |
---|
98 | } |
---|
99 | |
---|
100 | void Orient::lookAt(const Pt3D& X,const Pt3D& dir) |
---|
101 | { |
---|
102 | x=X; x.normalize(); |
---|
103 | y.vectorProduct(dir,x); |
---|
104 | z.vectorProduct(x,y); |
---|
105 | if ((!y.normalize()) || (!z.normalize())) |
---|
106 | { |
---|
107 | y.x=dir.y; y.y=dir.z; y.z=dir.x; |
---|
108 | z.vectorProduct(x,y); |
---|
109 | z.normalize(); |
---|
110 | } |
---|
111 | } |
---|
112 | |
---|
113 | // odleglosc 2d |
---|
114 | double d2(double x,double y) |
---|
115 | { |
---|
116 | double q=x*x+y*y; |
---|
117 | if (q<0) {if (Pt3D::report_errors) FramMessage("","d2()","sqrt domain error",3); return 0;} |
---|
118 | return sqrt(q); |
---|
119 | } |
---|
120 | |
---|
121 | Orient::Orient(const Matrix44& m) |
---|
122 | { |
---|
123 | x.x=m[0]; x.y=m[1]; x.z=m[2]; |
---|
124 | y.x=m[4]; y.y=m[5]; y.z=m[6]; |
---|
125 | z.x=m[8]; z.y=m[9]; z.z=m[10]; |
---|
126 | } |
---|
127 | |
---|
128 | void Orient::operator=(const Pt3D &rot) |
---|
129 | { |
---|
130 | *this=Orient_1; |
---|
131 | rotate(rot); |
---|
132 | } |
---|
133 | |
---|
134 | void Orient::rotate(const Pt3D &v) |
---|
135 | { |
---|
136 | double s,c; |
---|
137 | if (fabs(v.x)>0.0001) |
---|
138 | { |
---|
139 | s=sin(v.x); c=cos(v.x); |
---|
140 | rotate2D(s,c,x.y,x.z); |
---|
141 | rotate2D(s,c,y.y,y.z); |
---|
142 | rotate2D(s,c,z.y,z.z); |
---|
143 | } |
---|
144 | if (fabs(v.y)>0.0001) |
---|
145 | { |
---|
146 | s=sin(v.y); c=cos(v.y); |
---|
147 | rotate2D(s,c,x.x,x.z); |
---|
148 | rotate2D(s,c,y.x,y.z); |
---|
149 | rotate2D(s,c,z.x,z.z); |
---|
150 | } |
---|
151 | if (fabs(v.z)>0.0001) |
---|
152 | { |
---|
153 | s=sin(v.z); c=cos(v.z); |
---|
154 | rotate2D(s,c,x.x,x.y); |
---|
155 | rotate2D(s,c,y.x,y.y); |
---|
156 | rotate2D(s,c,z.x,z.y); |
---|
157 | } |
---|
158 | } |
---|
159 | |
---|
160 | void Orient::transform(Pt3D& target,const Pt3D &s) const |
---|
161 | { |
---|
162 | target.x=s.x*x.x+s.y*y.x+s.z*z.x; |
---|
163 | target.y=s.x*x.y+s.y*y.y+s.z*z.y; |
---|
164 | target.z=s.x*x.z+s.y*y.z+s.z*z.z; |
---|
165 | } |
---|
166 | |
---|
167 | void Orient::revTransform(Pt3D& target,const Pt3D &s) const |
---|
168 | { |
---|
169 | target.x=s.x*x.x+s.y*x.y+s.z*x.z; |
---|
170 | target.y=s.x*y.x+s.y*y.y+s.z*y.z; |
---|
171 | target.z=s.x*z.x+s.y*z.y+s.z*z.z; |
---|
172 | } |
---|
173 | |
---|
174 | void Orient::transform(Orient& target,const Orient& src) const |
---|
175 | { |
---|
176 | transform(target.x,src.x); |
---|
177 | transform(target.y,src.y); |
---|
178 | transform(target.z,src.z); |
---|
179 | } |
---|
180 | |
---|
181 | void Orient::revTransform(Orient& target,const Orient& src) const |
---|
182 | { |
---|
183 | revTransform(target.x,src.x); |
---|
184 | revTransform(target.y,src.y); |
---|
185 | revTransform(target.z,src.z); |
---|
186 | } |
---|
187 | |
---|
188 | void Orient::getAngles(Pt3D &angles) const |
---|
189 | { |
---|
190 | angles.getAngles(x,z); |
---|
191 | } |
---|
192 | |
---|
193 | bool Orient::normalize() |
---|
194 | { |
---|
195 | bool ret=1; |
---|
196 | y.vectorProduct(z,x); |
---|
197 | z.vectorProduct(x,y); |
---|
198 | if (!x.normalize()) ret=0; |
---|
199 | if (!z.normalize()) ret=0; |
---|
200 | if (!y.normalize()) ret=0; |
---|
201 | return ret; |
---|
202 | } |
---|
203 | |
---|
204 | Matrix44::Matrix44(const Orient &rot) |
---|
205 | { |
---|
206 | m[0]=rot.x.x; m[1]=rot.x.y; m[2]=rot.x.z; m[3]=0; |
---|
207 | m[4]=rot.y.x; m[5]=rot.y.y; m[6]=rot.y.z; m[7]=0; |
---|
208 | m[8]=rot.z.x; m[9]=rot.z.y; m[10]=rot.z.z; m[11]=0; |
---|
209 | m[12]=0; m[13]=0; m[14]=0; m[15]=1; |
---|
210 | } |
---|
211 | |
---|
212 | void Matrix44::operator+=(const Pt3D &) |
---|
213 | { |
---|
214 | |
---|
215 | } |
---|
216 | |
---|
217 | void Matrix44::operator*=(const Pt3D &) |
---|
218 | { |
---|
219 | } |
---|
220 | |
---|
221 | void Matrix44::operator*=(double sc) |
---|
222 | { |
---|
223 | } |
---|