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
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
|
/***************************************************************************
rmath.cpp - description
-------------------
begin : Mon Sep 27 1999
copyright : (C) 1999 by Andreas Mustun
email : andrew@ribbonsoft.com
***************************************************************************/
/****************************************************************************
** rmath.cpp 1998/08/27 A. Mustun RibbonSoft
**
** Copyright (C) 1998 RibbonSoft. All rights reserved.
**
*****************************************************************************/
#include <math.h>
#include "rmath.h"
#include "relement.h"
// Compare two float values:
//
// return: true: values are equal
// false: values are not equal
//
bool
mtCompFloat(float _v1, float _v2, float _tol)
{
float delta = _v2-_v1;
if(delta>-_tol && delta<_tol) return true;
else return false;
}
// Correct an angle (bring it to range of 0-360)
// this function returns the new angle
// is doesn't change the parameter
//
float
mtCorrAngle(float _ang)
{
float ret=_ang;
while(ret< 0.0) ret+=360.0;
while(ret>=360.0) ret-=360.0;
return ret;
}
// Correct an angle (bring it to range of 0-360)
// this function changes the parameter
//
void
mtCorrAngle(float* _ang)
{
while(*_ang< 0.0) *_ang+=360.0;
while(*_ang>=360.0) *_ang-=360.0;
}
// Test if angle _ang is beteen _startAngle and _endAngle:
// (in pos. direction)
//
bool
mtAngleIsBetween(float _ang, float _startAngle, float _endAngle)
{
bool ret=false;
if(_startAngle>_endAngle) {
if(_ang>=_startAngle || _ang<=_endAngle) ret=true;
}
else {
if(_ang>=_startAngle && _ang<=_endAngle) ret=true;
}
return(ret);
}
// Get Angle from point _cx, _cy
// to point _px, _py
//
float
mtGetAngle(float _x1, float _y1, float _x2, float _y2)
{
float xdist, ydist, angle;
xdist=_x2-_x1;
ydist=_y2-_y1;
if(mtCompFloat(xdist, 0.0)) {
if(_y2<_y1) angle=270.0;
else angle=90.0;
return angle;
}
if(mtCompFloat(ydist, 0.0)) {
if(_x2<_x1) angle=180.0;
else angle=0.0;
return angle;
}
angle = atan(ydist/xdist)*ARAD;
if(angle<0.0) angle*=-1.0;
if(_x2>_x1 && _y2>_y1) angle = 0.0 + angle; // 1. Quadrant
if(_x2<_x1 && _y2>_y1) angle = 180.0 - angle; // 2. Quadrant
if(_x2<_x1 && _y2<_y1) angle = 180.0 + angle; // 3. Quadrant
if(_x2>_x1 && _y2<_y1) angle = 360.0 - angle; // 4. Quadrant
return angle;
}
// Get Angle difference between _a1 and _a2
//
// (Which angle must I add to _a1 to get _a2)
//
float
mtGetAngleDiff(float _a1, float _a2)
{
float ret;
if(_a1>=_a2) _a2+=360.0;
ret = _a2-_a1;
if(ret>=360.0) ret=0.0;
return ret;
}
// Get Distance from point _cx, _cy
// to point _px, _py
//
float
mtGetDistance(float _x1, float _y1, float _x2, float _y2)
{
float xd2 = (_x2-_x1)*(_x2-_x1);
float yd2 = (_y2-_y1)*(_y2-_y1);
float d = xd2+yd2;
if(d<0.0) d*=-1.0;
return(sqrt(d));
}
// Rotate point _px/_py around center _cx/_cy with angle _angle
//
void
mtRotatePoint(float _cx, float _cy, float* _px, float* _py, float _angle)
{
float dist, // temporary distance
ang; // temporary angle
dist = mtGetDistance(_cx, _cy, *_px, *_py);
ang = mtCorrAngle(mtGetAngle(_cx, _cy, *_px, *_py)+_angle);
*_px = _cx+cos(ang/ARAD)*dist;
*_py = _cy+sin(ang/ARAD)*dist;
}
// Mirror point _px/_py on line (_x1,_y1 / _x2,_y2):
//
void
mtMirrorPoint(float _x1, float _y1,
float _x2, float _y2,
float* _px, float* _py)
{
float tmpDis, // temporary distance
tmpX, tmpY; // temporary coordinate
RElement tmpEl; // temporary element
float mirAngle; // mirroring angle (angle of axe +90.0)
tmpEl.createLine(_x1, _y1, _x2, _y2);
mirAngle = mtCorrAngle(tmpEl.getDirection1()+90.0);
tmpDis = tmpEl.getDistanceToPoint(*_px, *_py, true);
tmpX = *_px + cos(mirAngle/ARAD)*tmpDis*2.0;
tmpY = *_py + sin(mirAngle/ARAD)*tmpDis*2.0;
// Mirrored to wrong side:
//
if(tmpEl.getDistanceToPoint(tmpX, tmpY, true) >= 1.9*tmpDis) {
tmpX = *_px - cos(mirAngle/ARAD)*tmpDis*2.0;
tmpY = *_py - sin(mirAngle/ARAD)*tmpDis*2.0;
}
*_px = tmpX;
*_py = tmpY;
}
// Square:
//
float
mtSqr(float _v)
{
return _v*_v;
}
// EOF
|
