source: cpp/frams/util/3d.cpp @ 109

Last change on this file since 109 was 109, checked in by sz, 11 years ago

source reorganization (see README)
new feature added: part/joint shapes (see frams/_demos/part_shapes.cpp)

  • Property svn:eol-style set to native
File size: 4.4 KB
Line 
1// This file is a part of the Framsticks GDK library.
2// Copyright (C) 2002-2011  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
9Pt3D operator+(const Pt3D &p1,const Pt3D &p2) {return Pt3D(p1.x+p2.x,p1.y+p2.y,p1.z+p2.z);}
10Pt3D operator-(const Pt3D &p1,const Pt3D &p2) {return Pt3D(p1.x-p2.x,p1.y-p2.y,p1.z-p2.z);}
11
12Pt3D Pt3D_0(0,0,0);
13
14bool Pt3D::report_errors=true;
15
16double Pt3D::operator()() const
17{
18double q=x*x+y*y+z*z;
19if (q<0) {if (report_errors) FramMessage("Pt3D","operator()","sqrt domain error",3); return 0;}
20return sqrt(q);
21}
22
23bool Pt3D::normalize()
24{
25double len=length();
26if (fabs(len)<1e-50) {if (report_errors) FramMessage("Pt3D","normalize()","vector too small",1); x=1;y=0;z=0; return false;}
27operator/=(len);
28return true;
29}
30
31double Pt3D::distanceTo(const Pt3D& p) const
32{
33return sqrt((x-p.x)*(x-p.x)+(y-p.y)*(y-p.y)+(z-p.z)*(z-p.z));
34}
35
36double Pt3D::manhattanDistanceTo(const Pt3D& p) const
37{
38return fabs(x-p.x)+fabs(y-p.y)+fabs(z-p.z);
39}
40
41Orient Orient_1(Pt3D(1,0,0),Pt3D(0,1,0),Pt3D(0,0,1));
42
43// prosty obrot
44void rotate2D(double k,double &x,double &y)
45{double s=sin(k),c=cos(k);
46double t=c*x-s*y; y=s*x+c*y; x=t;}
47
48void rotate2D(double s,double c,double &x,double &y)
49{double t=c*x-s*y; y=s*x+c*y; x=t;}
50
51int Pt3D::getAngle(double dx,double dy,double &wyn)
52{
53if ((fabs(dx)+fabs(dy))<0.0001) return 0;
54wyn=atan2(dy,dx);
55return 1;
56}
57
58void Pt3D::getAngles(const Pt3D& X,const Pt3D& dir)
59{
60Pt3D    t1(X), t2(dir);
61if (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        }
67else
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        }
75rotate2D(-y,t2.x,t2.z);
76if (!getAngle(t2.z,-t2.y,x))
77        x=0;// to zly wynik ale dobrego nie ma :P
78}
79
80void Pt3D::getMin(const Pt3D& p)
81{
82if (p.x<x) x=p.x;
83if (p.y<y) y=p.y;
84if (p.z<z) z=p.z;
85}
86void Pt3D::getMax(const Pt3D& p)
87{
88if (p.x>x) x=p.x;
89if (p.y>y) y=p.y;
90if (p.z>z) z=p.z;
91}
92
93void Pt3D::vectorProduct(const Pt3D& a,const Pt3D& b)
94{
95x=a.y*b.z-a.z*b.y;
96y=a.z*b.x-a.x*b.z;
97z=a.x*b.y-a.y*b.x;
98}
99
100void Orient::lookAt(const Pt3D& X,const Pt3D& dir)
101{
102x=X; x.normalize();
103y.vectorProduct(dir,x);
104z.vectorProduct(x,y);
105if ((!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
114double d2(double x,double y)
115{
116double q=x*x+y*y;
117if (q<0) {if (Pt3D::report_errors) FramMessage("","d2()","sqrt domain error",3); return 0;}
118return sqrt(q);
119}
120
121Orient::Orient(const Matrix44& m)
122{
123x.x=m[0];  x.y=m[1];  x.z=m[2];
124y.x=m[4];  y.y=m[5];  y.z=m[6];
125z.x=m[8];  z.y=m[9];  z.z=m[10];
126}
127
128void Orient::operator=(const Pt3D &rot)
129{
130*this=Orient_1;
131rotate(rot);
132}
133
134void Orient::rotate(const Pt3D &v)
135{
136double s,c;
137if (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        }
144if (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        }
151if (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
160void Orient::transform(Pt3D& target,const Pt3D &s) const
161{
162target.x=s.x*x.x+s.y*y.x+s.z*z.x;
163target.y=s.x*x.y+s.y*y.y+s.z*z.y;
164target.z=s.x*x.z+s.y*y.z+s.z*z.z;
165}
166
167void Orient::revTransform(Pt3D& target,const Pt3D &s) const
168{
169target.x=s.x*x.x+s.y*x.y+s.z*x.z;
170target.y=s.x*y.x+s.y*y.y+s.z*y.z;
171target.z=s.x*z.x+s.y*z.y+s.z*z.z;
172}
173
174void Orient::transform(Orient& target,const Orient& src) const
175{
176transform(target.x,src.x);
177transform(target.y,src.y);
178transform(target.z,src.z);
179}
180
181void Orient::revTransform(Orient& target,const Orient& src) const
182{
183revTransform(target.x,src.x);
184revTransform(target.y,src.y);
185revTransform(target.z,src.z);
186}
187
188void Orient::getAngles(Pt3D &angles) const
189{
190angles.getAngles(x,z);
191}
192
193bool Orient::normalize()
194{
195bool ret=1;
196y.vectorProduct(z,x);
197z.vectorProduct(x,y);
198if (!x.normalize()) ret=0;
199if (!z.normalize()) ret=0;
200if (!y.normalize()) ret=0;
201return ret;
202}
203
204Matrix44::Matrix44(const Orient &rot)
205{
206m[0]=rot.x.x;  m[1]=rot.x.y;  m[2]=rot.x.z;  m[3]=0;
207m[4]=rot.y.x;  m[5]=rot.y.y;  m[6]=rot.y.z;  m[7]=0;
208m[8]=rot.z.x;  m[9]=rot.z.y;  m[10]=rot.z.z; m[11]=0;
209m[12]=0;       m[13]=0;       m[14]=0;       m[15]=1;
210}
211
212void Matrix44::operator+=(const Pt3D &)
213{
214
215}
216
217void Matrix44::operator*=(const Pt3D &)
218{
219}
220
221void Matrix44::operator*=(double sc)
222{
223}
Note: See TracBrowser for help on using the repository browser.