1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
|
// -*- C++ -*-
/*
* Simple MultimEdia LiTerator(SMELT)
* by Chris Xiong 2015
* Math header & implementation
*
* WARNING: This library is in development and interfaces would be very
* unstable.
*
*/
#ifndef SMMATH_H
#define SMMATH_H
#include <cmath>
#include <cstddef>
#define sqr(x) ((x)*(x))
#define EPS 1e-8
#ifndef PI
#define PI 3.14159265358979323846f
#endif
template<class T>const T& min(const T& a,const T& b){return a<b?a:b;}
template<class T>const T& max(const T& a,const T& b){return a>b?a:b;}
class smMath
{
public:
static double deg2rad(double deg){return deg/180.*PI;}
static double rad2deg(double rad){return rad/PI*180.;}
static double clamprad(double a){while(a<0)a+=2*PI;while(a>2*PI)a-=2*PI;return a;}
static double clampdeg(double a){while(a<0)a+=360.;while(a>360)a-=360.;return a;}
};
class smvec2d
{
public:
double x,y;
smvec2d(double _x,double _y){x=_x;y=_y;}
smvec2d(){x=y=.0;}
double l(){return sqrt(sqr(x)+sqr(y));}
void normalize(){double L=l();if(L<EPS)return;x/=L;y/=L;}
smvec2d getNormalized(){double L=l();if(L<EPS)return smvec2d(0,0);return smvec2d(x/L,y/L);}
void swap(){double t=x;x=y;y=t;}
void rotate(double rad){double tx=x*cos(rad)+y*sin(rad),ty=y*cos(rad)-x*sin(rad);x=tx,y=ty;}
smvec2d getRotate(double rad){double tx=x*cos(rad)+y*sin(rad),ty=y*cos(rad)-x*sin(rad);return smvec2d(tx,ty);}
friend smvec2d operator -(smvec2d a,smvec2d b){return smvec2d(a.x-b.x,a.y-b.y);}
friend smvec2d operator +(smvec2d a,smvec2d b){return smvec2d(a.x+b.x,a.y+b.y);}
friend double operator |(smvec2d a,smvec2d b){return a.x*b.x+a.y*b.y;}
friend double operator *(smvec2d a,smvec2d b){return a.x*b.y-b.x*a.y;}
friend smvec2d operator *(double a,smvec2d b){return smvec2d(a*b.x,a*b.y);}
friend smvec2d operator *(smvec2d a,double b){return smvec2d(b*a.x,b*a.y);}
friend double operator ^(smvec2d a,smvec2d b){return (a|b)/a.l()/b.l();}
};
class smvec3d
{
public:
double x,y,z;
smvec3d(double _x,double _y,double _z){x=_x;y=_y;z=_z;}
smvec3d(smvec2d a,double _z=.0){x=a.x;y=a.y;z=_z;}
smvec3d(){x=y=z=.0;}
double l(){return sqrt(sqr(x)+sqr(y)+sqr(z));}
void normalize(){double L=l();if(L<EPS)return;x/=L;y/=L;z/=L;}
smvec3d getNormalized(){double L=l();if(L<EPS)return smvec3d(0,0,0);return smvec3d(x/L,y/L,z/L);}
friend smvec3d operator -(smvec3d a,smvec3d b){return smvec3d(a.x-b.x,a.y-b.y,a.z-b.z);}
friend smvec3d operator +(smvec3d a,smvec3d b){return smvec3d(a.x+b.x,a.y+b.y,a.z+b.z);}
friend double operator |(smvec3d a,smvec3d b){return a.x*b.x+a.y*b.y+a.z*b.z;}
friend smvec3d operator *(smvec3d a,smvec3d b){return smvec3d(a.y*b.z-a.z*b.y,a.z*b.x-a.x*b.z,a.x*b.y-a.y*b.x);}
friend smvec3d operator *(double a,smvec3d b){return smvec3d(a*b.x,a*b.y,a*b.z);}
friend smvec3d operator *(smvec3d a,double b){return smvec3d(b*a.x,b*a.y,b*a.z);}
friend double operator ^(smvec3d a,smvec3d b){return (a|b)/a.l()/b.l();}
};
class smvec4d
{
public:
double x,y,z,w;
smvec4d(double _x,double _y,double _z,double _w){x=_x;y=_y;z=_z;w=_w;}
smvec4d(smvec3d a,double _w=.0){x=a.x;y=a.y;z=a.z;w=_w;}
smvec4d(){x=y=z=w=.0;}
double l(){return sqrt(sqr(x)+sqr(y)+sqr(z)+sqr(w));}
void normalize(){double L=l();if(L<EPS)return;x/=L;y/=L;z/=L;w/=L;}
smvec4d getNormalized(){double L=l();if(L<EPS)return smvec4d(0,0,0,0);return smvec4d(x/L,y/L,z/L,w/L);}
friend smvec4d operator -(smvec4d a,smvec4d b){return smvec4d(a.x-b.x,a.y-b.y,a.z-b.z,a.w-b.w);}
friend smvec4d operator +(smvec4d a,smvec4d b){return smvec4d(a.x+b.x,a.y+b.y,a.z+b.z,a.w+b.w);}
friend double operator |(smvec4d a,smvec4d b){return a.x*b.x+a.y*b.y+a.z*b.z+a.w*b.w;}
//Note: this doesn't do a real 4d cross product.
friend smvec4d operator *(smvec4d a,smvec4d b){return smvec4d(a.y*b.z-a.z*b.y,a.z*b.x-a.x*b.z,a.x*b.y-a.y*b.x,1);}
friend smvec4d operator *(double a,smvec4d b){return smvec4d(a*b.x,a*b.y,a*b.z,a*b.w);}
friend smvec4d operator *(smvec4d a,double b){return smvec4d(b*a.x,b*a.y,b*a.z,b*a.w);}
friend double operator ^(smvec4d a,smvec4d b){return (a|b)/a.l()/b.l();}
};
class smMatrix
{
public:
double m[16];
/* sf 0 1 2 3
* 0 00 04 08 12
* 1 01 05 09 13
* 2 02 06 10 14
* 3 03 07 11 15
*/
smMatrix(){for(int i=0;i<16;++i)m[i]=.0;}
smMatrix(const smMatrix ©){for(int i=0;i<16;++i)m[i]=copy.m[i];}
double* operator [](int s){if(s>=0&&s<4)return m+s*4;else return NULL;}
void clear(){for(int i=0;i<16;++i)m[i]=.0;}
void loadIdentity(){clear();m[0]=m[5]=m[10]=m[15]=1.;}
void translate(double x,double y,double z)
{
smMatrix tmp;tmp.loadIdentity();
tmp.m[12]=x;tmp.m[13]=y;tmp.m[14]=z;
*this=*this*tmp;
}
void rotate(double a,double x,double y,double z)
{
if(smvec3d(x,y,z).l()<=EPS)return;
if(fabs(smvec3d(x,y,z).l()-1)>EPS)
{
smvec3d a(x,y,z);a.normalize();
x=a.x;y=a.y;z=a.z;
}
smMatrix tmp;
double c=cos(a),s=sin(a);
tmp.m[ 0]=x*x*(1-c)+c;
tmp.m[ 4]=x*y*(1-c)-z*s;
tmp.m[ 8]=x*z*(1-c)+y*s;
tmp.m[ 1]=y*x*(1-c)+z*s;
tmp.m[ 5]=y*y*(1-c)+c;
tmp.m[ 9]=y*z*(1-c)-x*s;
tmp.m[ 2]=x*z*(1-c)-y*s;
tmp.m[ 6]=y*z*(1-c)+x*s;
tmp.m[10]=z*z*(1-c)+c;
tmp.m[15]=1;
*this=*this*tmp;
}
void lookat(smvec3d eye,smvec3d at,smvec3d up)
{
smvec3d f=at-eye;f.normalize();up.normalize();
smvec3d s=f*up;smvec3d u=s.getNormalized()*f;
m[0]= s.x;m[4]= s.y;m[ 8]= s.z;m[12]=0;
m[1]= u.x;m[5]= u.y;m[ 9]= u.z;m[13]=0;
m[2]=-f.x;m[6]=-f.y;m[10]=-f.z;m[14]=0;
m[3]= 0;m[7]= 0;m[11]= 0;m[15]=1;
}
friend smMatrix operator *(smMatrix a,smMatrix b)
{
smMatrix ret;
for(int i=0;i<4;++i)
for(int j=0;j<4;++j)
for(int k=0;k<4;++k)
ret[j][i]+=a[k][i]*b[j][k];
return ret;
}
friend smvec3d operator *(smMatrix a,smvec3d b)
{
return smvec3d(a[0][0]*b.x+a[1][0]*b.y+a[2][0]*b.z,
a[0][1]*b.x+a[1][1]*b.y+a[2][1]*b.z,
a[0][2]*b.x+a[1][2]*b.y+a[2][2]*b.z);
}
friend smvec4d operator *(smMatrix a,smvec4d b)
{
return smvec4d(a[0][0]*b.x+a[1][0]*b.y+a[2][0]*b.z+a[3][0]*b.w,
a[0][1]*b.x+a[1][1]*b.y+a[2][1]*b.z+a[3][1]*b.w,
a[0][2]*b.x+a[1][2]*b.y+a[2][2]*b.z+a[3][2]*b.w,
a[0][3]*b.x+a[1][3]*b.y+a[2][3]*b.z+a[3][3]*b.w);
}
};
#endif
|
