source: cpp/gdk/3d.cpp @ 66

Last change on this file since 66 was 66, checked in by Maciej Komosinski, 13 years ago

set 'eol-style' to 'native'

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