source: cpp/frams/genetics/fF/conv_fF.cpp @ 185

Last change on this file since 185 was 185, checked in by Maciej Komosinski, 10 years ago

Avoid "atan2: domain error"

  • Property svn:eol-style set to native
File size: 8.6 KB
Line 
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 "conv_fF.h"
6#include "fF_genotype.h"
7#include <frams/model/model.h>
8#include <common/nonstd_stl.h>
9#include <common/Convert.h>
10
11GenoConv_fF0::GenoConv_fF0()
12{
13        name = "7-value Foraminifera encoding";
14        in_format = 'F';
15        out_format = '0';
16        mapsupport = 0;
17        cosines = new double[fF_LATITUDE_NUM];
18        sines = new double[fF_LATITUDE_NUM];
19        precompute_cos_and_sin();
20}
21
22GenoConv_fF0::~GenoConv_fF0()
23{
24        delete[] cosines;
25        delete[] sines;
26}
27
28SString GenoConv_fF0::convert(SString &in, MultiMap *map)
29{
30        fF_growth_params gp;
31        if (!gp.load(in)) //invalid input genotype?
32                return ""; //so we return an invalid f0 genotype
33
34        double div_radius_length = 1; //div_radius_length=1 or kx=ky=kz=1
35        double radius = 1;
36
37        Model m;
38        m.open();
39
40        m.vis_style = "foram"; //dedicated visual look for Foraminifera
41
42        // subsequent parts (chambers) are placed relative to the previous part's orientation and location
43        Part *p1, *p2;
44
45        fF_chamber3d **chambers = new fF_chamber3d*[gp.number_of_chambers];
46
47        for (int i = 0; i < gp.number_of_chambers; i++)
48                createSphere(i, chambers, radius, div_radius_length, gp.translation, gp.angle1, gp.angle2, gp.scalex, gp.scaley, gp.scalez);
49
50        p1 = m.addNewPart(Part::SHAPE_ELLIPSOID);
51        p1->p = Pt3D(chambers[0]->centerX, chambers[0]->centerY, chambers[0]->centerZ);
52
53
54        for (int i = 1; i < gp.number_of_chambers; i++, p1 = p2)
55        {
56                p2 = m.addNewPart(Part::SHAPE_ELLIPSOID);
57                p2->scale = p1->scale.entrywiseProduct(Pt3D(gp.scalex, gp.scaley, gp.scalez)); //each part's scale is its predecessor's scale * scaling
58
59                p2->p = Pt3D(chambers[i]->centerX, chambers[i]->centerY, chambers[i]->centerZ);
60
61                m.addNewJoint(p1, p2, Joint::SHAPE_SOLID); //all parts must be connected
62        }
63
64        for (int i = 0; i < gp.number_of_chambers; i++)
65                delete chambers[i];
66        delete[]chambers;
67
68        m.close();
69        return m.getF0Geno().getGene();
70}
71
72void GenoConv_fF0::createSphere(int which, fF_chamber3d **chambers, double radius_, double div_radius_length_, double div_vector_length_,
73        double alpha_, double gamma_, double kx_, double ky_, double kz_)
74{
75        chambers[which] = new fF_chamber3d(0.0f, 0.0f, 0.0f,
76                (float)radius_, (float)radius_ * (float)kx_, 0.0f, 0.0f,
77                (float)(radius_ * div_vector_length_), 0.0f, 0.0f, 0.0f, 0.0f);
78        if (which == 0)
79                chambers[which]->points = generate_points(chambers[which], which, kx_, ky_, kz_);
80        if (which > 0) {
81                /* old radius */
82                double radiusOld, radius;
83                radiusOld = chambers[which - 1]->radius;
84                radius = div_radius_length_ * radiusOld;
85                /* new growth vector length */
86                double len = radius * div_vector_length_;
87                if (radius < fF_TOO_LITTLE) {
88                        radius = fF_TOO_LITTLE;
89                        if (fabs(len) >(fF_TOO_MUCH * radius)) {
90                                len = ((len < 0) ? (-1) : 1) * fF_TOO_MUCH * radius;
91                        }
92                }
93                if (len == 0) {
94                        len = -0.0000001;
95                }
96
97                /* aperture of the previous chamber */
98                double pzx = chambers[which - 1]->holeX;
99                double pzy = chambers[which - 1]->holeY;
100                double pzz = chambers[which - 1]->holeZ;
101
102                //center of the previous chamber
103                double pcx = chambers[which - 1]->centerX;
104                double pcy = chambers[which - 1]->centerY;
105                double pcz = chambers[which - 1]->centerZ;
106
107                /* aperture of the next to last chamber */
108                double ppx;
109                double ppy;
110                double ppz;
111
112                if (which == 1)
113                {
114                        ppx = pcx;
115                        ppy = pcy;
116                        ppz = pcz;
117                }
118                else
119                {
120                        ppx = chambers[which - 2]->holeX;
121                        ppy = chambers[which - 2]->holeY;
122                        ppz = chambers[which - 2]->holeZ;
123                }
124
125                double pzxprim = pzx - ppx;
126                double pzyprim = pzy - ppy;
127                double angle;
128
129                angle = Convert::atan_2(pzyprim, pzxprim);
130                double alpha = angle - alpha_;
131
132
133                double gamma = chambers[which - 1]->phi + gamma_;
134
135                /* x */
136                double wx = len * cos(alpha);
137                /* y */
138                double wy = len * sin(alpha);
139                /* y */
140                double wz = len * sin(alpha) * sin(gamma);
141
142                /*center of the new sphere*/
143                double x = pzx + wx;
144                double y = pzy + wy;
145                double z = pzz + wz;
146
147                chambers[which]->centerX = (float)x;
148                chambers[which]->centerY = (float)y;
149                chambers[which]->centerZ = (float)z;
150                chambers[which]->radius = (float)radius;
151                chambers[which]->vectorTfX = (float)wx;
152                chambers[which]->vectorTfY = (float)wy;
153                chambers[which]->vectorTfZ = (float)wz;
154                chambers[which]->beta = (float)alpha;
155                chambers[which]->phi = (float)gamma;
156
157                chambers[which]->points = generate_points(chambers[which], which, kx_, ky_, kz_);
158                search_hid(which, chambers, kx_, ky_, kz_);
159                int pun;
160                pun = find_hole(which, pzx, pzy, pzz, chambers, kx_, ky_, kz_);
161
162                chambers[which]->holeX = (float)chambers[which]->points[pun].x;
163                chambers[which]->holeY = (float)chambers[which]->points[pun].y;
164                chambers[which]->holeZ = (float)chambers[which]->points[pun].z;
165        }
166}
167
168void GenoConv_fF0::precompute_cos_and_sin()
169{
170        int i;
171        double pi = acos(-1.0);
172        double angle = pi / (((double)fF_LATITUDE_NUM)*0.5);
173        for (i = 0; i < fF_LATITUDE_NUM; i++)
174        {
175                cosines[i] = cos((double)i * angle);
176                sines[i] = sin((double)i * angle);
177        }
178}
179
180fF_point* GenoConv_fF0::generate_points(fF_chamber3d *chamber, int which, double kx_, double ky_, double kz_)
181{
182        float radius = chamber->radius;
183        float cenx = chamber->centerX;
184        float ceny = chamber->centerY;
185        float cenz = chamber->centerZ;
186
187        double maxX = 0;
188        double maxY = 0;
189        double minX = 0;
190        double minY = 0;
191        double minZ = 0;
192
193        double kx = 1;
194        double ky = 1;
195        double kz = 1;
196
197        if (which > 0)
198        {
199                for (int kt = 1; kt < (which + 1); kt++)
200                {
201                        kx = kx * kx_;
202                        ky = ky * ky_;
203                        kz = kz * kz_;
204                }
205        }
206
207        bool all_k_ones = kx_ == 1 && ky_ == 1 && kz_ == 1;
208
209        double rx = all_k_ones ? radius : kx;
210        double ry = all_k_ones ? radius : ky;
211        double rz = all_k_ones ? radius : kz;
212
213        fF_point *points = new fF_point[fF_SIZE];
214
215        for (int i = 0; i < fF_LONGITUDE_NUM; i++)
216        {
217                double y = ceny + ry * cosines[i];
218
219                for (int j = 0; j < fF_LATITUDE_NUM; j++)
220                {
221                        double x = cenx + rx * cosines[j] * sines[i];
222                        double z = cenz + rz * sines[j] * sines[i];
223                        fF_point &p = points[(i * fF_LATITUDE_NUM) + j];
224                        p.x = x;
225                        p.y = y;
226                        p.z = z;
227                        p.inside = false;
228
229                        if (x < minX) minX = x;
230                        if (x > maxX) maxX = x;
231                        if (y < minY) minY = y;
232                        if (y > maxY) maxY = y;
233
234                        if (z < minZ) minZ = z;
235                }
236        }
237        return points;
238
239}
240
241double GenoConv_fF0::dist(double x1, double y1, double z1, double x2, double y2, double z2)
242{
243        return sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1) + (z2 - z1)*(z2 - z1));
244}
245
246void GenoConv_fF0::search_hid(int nr, fF_chamber3d **spheres, double kx_, double ky_, double kz_)
247{
248        double kxsq = kx_*kx_;
249        double kysq = ky_*ky_;
250        double kzsq = kz_*kz_;
251
252        for (int i = 0; i < nr; i++)
253        {
254                double srX0 = spheres[i]->centerX;
255                double srY0 = spheres[i]->centerY;
256                double srZ0 = spheres[i]->centerZ;
257
258                double radsq = spheres[i]->radius * spheres[i]->radius;
259
260                double a2 = kx_ != 1 ? kxsq : radsq;
261                double b2 = ky_ != 1 ? kysq : radsq;
262                double c2 = kzsq * radsq;
263
264                for (int j = 0; j < fF_AMOUNT; j++)
265                {
266                        fF_point &p = spheres[nr]->points[j];
267
268                        double up1 = (p.x - srX0) * (p.x - srX0);
269                        double up2 = (p.y - srY0) * (p.y - srY0);
270                        double up3 = (p.z - srZ0) * (p.z - srZ0);
271
272                        double exp1 = up1 / a2;
273                        double exp2 = up2 / b2;
274                        double exp3 = up3 / c2;
275
276                        double result = exp1 + exp2 + exp3;
277
278                        if (result < fF_THICK_RATIO)
279                        {
280                                p.inside = true;
281                        }
282                }
283        }
284}
285
286int GenoConv_fF0::find_hole(int which, double x, double y, double z, fF_chamber3d **chambers, double kx_, double ky_, double kz_)
287{
288        int found = -1;
289        double distsq_found;
290
291        double kxsq = kx_*kx_;
292        double kysq = ky_*ky_;
293        double kzsq = kz_*kz_;
294
295        for (int i = 0; i < fF_AMOUNT; i++)
296        {
297                fF_point &p = chambers[which]->points[i];
298                if (!p.inside) //it is not inside another chamber
299                {
300                        double distancesq = (p.x - x)*(p.x - x) + (p.y - y)*(p.y - y) + (p.z - z)*(p.z - z);
301                        if (found < 0)
302                        {
303                                found = i;
304                                distsq_found = distancesq;
305                        }
306                        if (distancesq < distsq_found)
307                        {
308                                if (which != 0)
309                                {
310                                        bool good = true;
311                                        for (int j = 0; j < which && good; j++)
312                                        {
313                                                double srX0 = chambers[j]->centerX;
314                                                double srY0 = chambers[j]->centerY;
315                                                double srZ0 = chambers[j]->centerZ;
316
317                                                double radsq = chambers[j]->radius * chambers[j]->radius;
318
319                                                double a2 = kxsq * radsq;
320                                                double b2 = kysq * radsq;
321                                                double c2 = kzsq * radsq;
322
323                                                double up1 = (p.x - srX0) * (p.x - srX0);
324                                                double up2 = (p.y - srY0) * (p.y - srY0);
325                                                double up3 = (p.z - srZ0) * (p.z - srZ0);
326
327                                                double exp1 = up1 / a2;
328                                                double exp2 = up2 / b2;
329                                                double exp3 = up3 / c2;
330
331                                                double result = exp1 + exp2 + exp3;
332                                                if (result < 1.0)
333                                                {
334                                                        good = false;
335                                                }
336                                        }
337                                        if (good)
338                                        {
339                                                found = i;
340                                                distsq_found = distancesq;
341                                        }
342                                }
343                        }
344                }
345        }
346
347        return found;
348}
Note: See TracBrowser for help on using the repository browser.