Index: cpp/frams/genetics/fF/conv_fF.cpp
===================================================================
--- cpp/frams/genetics/fF/conv_fF.cpp	(revision 157)
+++ cpp/frams/genetics/fF/conv_fF.cpp	(revision 174)
@@ -8,44 +8,340 @@
 #include <common/nonstd_stl.h>
 
-GenoConv_fF0::GenoConv_fF0()
-{
-	name = "7-value Foraminifera encoding";
-	in_format = 'F';
-	out_format = '0';
-	mapsupport = 0;
-}
-
-SString GenoConv_fF0::convert(SString &in, MultiMap *map)
-{
-	fF_growth_params gp;
-	if (!gp.load(in)) //invalid input genotype?
-		return ""; //so we return an invalid f0 genotype
-	Model m;
-	m.open();
-	// subsequent parts (chambers) are placed relative to the previous part's orientation and location
-	Part *p1, *p2;
-	p1 = m.addNewPart(Part::SHAPE_ELLIPSOID); //initial part
-	Pt3D scaling(gp.scalex, gp.scaley, gp.scalez);
-	p1->scale = scaling;
-
-	Orient rotation = Orient_1; //must be initialized explicitly because the default Orient constructor does not initialize anything
-	rotation.rotate(Pt3D(0, gp.angle1, gp.angle2)); //assumed rotation around x is 0, which is limiting if we use assymetrical shapes (i.e., not spheres). But the original model had only two angles...
-
-	for (int i = 1; i < gp.number_of_chambers; i++, p1 = p2)
-	{
-		p2 = m.addNewPart(Part::SHAPE_ELLIPSOID);
-		p2->scale = p1->scale.entrywiseProduct(scaling); //each part's scale is its predecessor's scale * scaling
-
-		p2->setOrient(p1->o.transform(rotation)); //rotation transformed by p1 orientation
-
-		// Part's tip (offset from the center) is the point at (scale.x,0,0) in part's local coordinates.
-		// Get the offset in global coordinates by transforming it by the part's orientation.
-		p2->p = p1->p
-			+ p1->o.transform(Pt3D(p1->scale.x*gp.translation,0,0))  // p1's tip transformed
-			+ p2->o.transform(Pt3D(p2->scale.x*gp.translation,0,0)); // p2's tip transformed
-
-		m.addNewJoint(p1, p2, Joint::SHAPE_SOLID); //all parts must be connected
-	}
-	m.close();
-	return m.getF0Geno().getGene();
-}
+GenoConv_fF0::GenoConv_fF0() {
+    name = "7-value Foraminifera encoding";
+    in_format = 'F';
+    out_format = '0';
+    mapsupport = 0;
+    cosines = new double[LATITUDE_NUM];
+    sines = new double[LATITUDE_NUM];  
+    fill_cos_and_sin();
+}
+
+GenoConv_fF0::~GenoConv_fF0() {
+    delete []cosines;
+    delete []sines;
+}
+
+SString GenoConv_fF0::convert(SString &in, MultiMap *map) {
+    fF_growth_params gp;
+    if (!gp.load(in)) //invalid input genotype?
+        return ""; //so we return an invalid f0 genotype
+
+    double div_radius_length = 1;//div_radius_lenght=1 or kx=ky=kz=1
+    double radius = 1;
+
+    Model m;
+    m.open();
+    // subsequent parts (chambers) are placed relative to the previous part's orientation and location
+    Part *p1, *p2;
+
+    fF_chamber3d **chambers = new fF_chamber3d*[gp.number_of_chambers];
+
+    for (int i = 0; i < gp.number_of_chambers; i++) {
+        createSphere(i, chambers, radius, div_radius_length, gp.translation, gp.angle1, gp.angle2, gp.scalex, gp.scaley, gp.scalez);
+    }
+
+    p1 = m.addNewPart(Part::SHAPE_ELLIPSOID);
+    p1->p = Pt3D(chambers[0]->centerX, chambers[0]->centerY, chambers[0]->centerZ);
+
+
+    for (int i = 1; i < gp.number_of_chambers; i++, p1 = p2) {
+        p2 = m.addNewPart(Part::SHAPE_ELLIPSOID);
+        p2->scale = p1->scale.entrywiseProduct(Pt3D(gp.scalex, gp.scaley, gp.scalez)); //each part's scale is its predecessor's scale * scaling
+
+        p2->p = Pt3D(chambers[i]->centerX, chambers[i]->centerY, chambers[i]->centerZ);
+
+        m.addNewJoint(p1, p2, Joint::SHAPE_SOLID); //all parts must be connected
+    }
+
+    for (int i = 0; i < gp.number_of_chambers; i++)
+        delete chambers[i];
+    delete []chambers;
+
+    m.close();
+    return m.getF0Geno().getGene();
+}
+
+void GenoConv_fF0::createSphere(int which, fF_chamber3d **chambers, double radius_, double div_radius_length_, double div_vector_length_,
+        double alpha_, double gamma_, double kx_, double ky_, double kz_) 
+{    
+    chambers[which]=new fF_chamber3d(0.0f, 0.0f, 0.0f, 
+            (float) radius_, (float) radius_ * (float) kx_, 0.0f, 0.0f, 
+            (float) (radius_ * div_vector_length_), 0.0f, 0.0f, 0.0f, 0.0f); 
+    if (which == 0)
+        chambers[which]->points = generate_points(chambers[which], which, kx_, ky_, kz_);
+    if (which > 0) {
+        /* old radius */
+        double radiusOld, radius;
+        radiusOld = chambers[which - 1]->radius;
+        radius = div_radius_length_ * radiusOld;
+        /* new growth vector length */
+        double len = radius * div_vector_length_;
+        if (radius < TOO_LITTLE) {
+            radius = TOO_LITTLE;
+            if (fabs(len) > (TOO_MUCH * radius)) {
+                len = ((len < 0) ? (-1) : 1) * TOO_MUCH * radius;
+            }
+        }
+        if (len == 0) {
+            len = -0.0000001;
+        }
+
+        /* aperture of the previous chamber */
+        double pzx = chambers[which - 1]->holeX;
+        double pzy = chambers[which - 1]->holeY;
+        double pzz = chambers[which - 1]->holeZ;
+
+        //center of the previous chamber
+        double pcx = chambers[which - 1]->centerX;
+        double pcy = chambers[which - 1]->centerY;
+        double pcz = chambers[which - 1]->centerZ;
+
+        /* aperture of the next to last chamber */
+        double ppx;
+        double ppy;
+        double ppz;
+
+        if (which == 1) {
+            ppx = pcx;
+            ppy = pcy;
+            ppz = pcz;
+        } else {
+            ppx = chambers[which - 2]->holeX;
+            ppy = chambers[which - 2]->holeY;
+            ppz = chambers[which - 2]->holeZ;
+        }
+
+        double pzxprim = pzx - ppx;
+        double pzyprim = pzy - ppy;
+        double angle;
+
+        angle = atan2(pzyprim, pzxprim);
+        double alpha = angle - alpha_;
+
+
+        double gamma = chambers[which - 1]->phi + gamma_;
+
+        /* x */
+        double wx = len * cos(alpha);
+        /* y */
+        double wy = len * sin(alpha);
+        /* y */
+        double wz = len * sin(alpha) * sin(gamma);
+
+        /*center of the new sphere*/
+        double x = pzx + wx;
+        double y = pzy + wy;
+        double z = pzz + wz;
+
+        chambers[which]->centerX = (float) x;
+        chambers[which]->centerY = (float) y;
+        chambers[which]->centerZ = (float) z;
+        chambers[which]->radius= (float) radius;
+        chambers[which]->vectorTfX = (float) wx;
+        chambers[which]->vectorTfY = (float) wy;
+        chambers[which]->vectorTfZ = (float) wz;
+        chambers[which]->beta = (float) alpha;
+        chambers[which]->phi = (float) gamma;
+
+        chambers[which]->points = generate_points(chambers[which], which, kx_,  ky_,  kz_);
+        search_hid(which, chambers, kx_,  ky_,  kz_);
+        int pun;
+        pun = find_hole(which, pzx, pzy, pzz, chambers, kx_,  ky_,  kz_);
+
+        chambers[which]->holeX = (float) chambers[which]->points[pun][0];
+        chambers[which]->holeY = (float) chambers[which]->points[pun][1];
+        chambers[which]->holeZ = (float) chambers[which]->points[pun][2];
+    }
+}
+
+void GenoConv_fF0::fill_cos_and_sin() {
+    int i;
+    double pi = acos(-1.0);
+    double angle = pi / (((double) LATITUDE_NUM)*0.5);
+    for (i = 0; i < LATITUDE_NUM; i++) {
+        cosines[i] = cos((double) i * angle);
+        sines[i] = sin((double) i * angle);
+    }
+}
+
+double** GenoConv_fF0::generate_points(fF_chamber3d *chamber, int which, double kx_, double ky_, double kz_) {
+    float radius = chamber->radius;
+    float cenx = chamber->centerX;
+    float ceny = chamber->centerY;
+    float cenz = chamber->centerZ;
+
+    double maxX = 0;
+    double maxY = 0;
+    double minX = 0;
+    double minY = 0;
+    double minZ = 0;
+
+    double kx = 1;
+    double ky = 1;
+    double kz = 1;
+
+    if (which > 0) {
+        for (int kt = 1; kt < (which + 1); kt++) {
+            kx = kx * kx_;
+            ky = ky * ky_;
+            kz = kz * kz_;
+        }
+    }
+
+    int i, j;
+    double x, y, z;
+    
+    double **points = new double*[SIZE];
+    for (int i = 0; i < SIZE; i++) {
+        points[i] = new double[4];
+    }
+    
+    for (i = 0; i < LONGITUDE_NUM; i++) {
+        if (kx_ == 1 && ky_ == 1 && kz_ == 1) {
+            y = ceny + radius * cosines[i];
+        }
+        else {
+            y = ceny + ky * cosines[i];
+        }
+        for (j = 0; j < LATITUDE_NUM; j++) {
+            if (kx_ == 1 && ky_ == 1 && kz_ == 1) {
+                points[(i * LATITUDE_NUM) + j][0] = x = cenx + radius * cosines[j] * sines[i];
+                points[(i * LATITUDE_NUM) + j][1] = y;
+                points[(i * LATITUDE_NUM) + j][2] = z = cenz + radius * sines[j] * sines[i];
+            } else {
+                points[(i * LATITUDE_NUM) + j][0] = x = cenx + kx * cosines[j] * sines[i];
+                points[(i * LATITUDE_NUM) + j][1] = y;
+                points[(i * LATITUDE_NUM) + j][2] = z = cenz + kz * sines[j] * sines[i];
+            }
+
+            points[(i * LATITUDE_NUM) + j][3] = 1.0;
+            if (x < minX) minX = x;
+            if (x > maxX) maxX = x;
+            if (y < minY) minY = y;
+            if (y > maxY) maxY = y;
+
+            if (z < minZ) minZ = z;
+        };
+    };
+    return points;
+
+}
+
+double GenoConv_fF0::dist(double x1, double y1, double z1, double x2, double y2, double z2) {
+    return sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1) + (z2 - z1)*(z2 - z1));
+}
+
+void GenoConv_fF0::search_hid(int nr, fF_chamber3d **spheres, double kx_, double ky_, double kz_) {
+
+    int i, j;
+    if (nr != 0) {
+        for (i = 0; i < nr; i++) {
+            for (j = 0; j < AMOUNT; j++) {
+                double X = spheres[nr]->points[j][0];
+                double Y = spheres[nr]->points[j][1];
+                double Z = spheres[nr]->points[j][2];
+
+                double srX0 = spheres[i]->centerX;
+                double srY0 = spheres[i]->centerY;
+                double srZ0 = spheres[i]->centerZ;
+
+                double a2;
+                double b2;
+                double c2;
+
+                if (kx_ != 1) {
+                    a2 = (kx_ * kx_);
+                } else {
+                    a2 = (spheres[i]->radius * spheres[i]->radius);
+                }
+
+                if (ky_ != 1) {
+                    b2 = (ky_ * ky_);
+
+                } else {
+                    b2 = (spheres[i]->radius * spheres[i]->radius);
+                }
+
+                c2 = (kz_ * spheres[i]->radius) * (kz_ * spheres[i]->radius);
+
+                double up1 = (X - srX0) * (X - srX0);
+                double up2 = (Y - srY0) * (Y - srY0);
+                double up3 = (Z - srZ0) * (Z - srZ0);
+
+                double exp = up1 / a2;
+                double exp2 = up2 / b2;
+                double exp3 = up3 / c2;
+
+                double result = exp + exp2 + exp3;
+
+                if (result < (THICK_RATIO)
+                        ) {
+                    spheres[nr]->points[j][3] = 0;
+                }
+            }
+        }
+    }
+}
+
+int GenoConv_fF0::find_hole(int which, double x, double y, double z, fF_chamber3d **chambers, double kx_, double ky_, double kz_) {
+    int i;
+    double distance;
+    int found = 0;
+    double dist_found = 0;
+    int first = 1;
+
+    for (i = 0; i < AMOUNT; i++) {
+        if (chambers[which]->points[i][3] != 0) //nie jest wewnatrz inne komory
+        {
+            distance = sqrt((chambers[which]->points[i][0] - x)*(chambers[which]->points[i][0] - x) +
+                    (chambers[which]->points[i][1] - y)*(chambers[which]->points[i][1] - y)+
+                    (chambers[which]->points[i][2] - z)*(chambers[which]->points[i][2] - z));
+            if (first != 0) {
+                found = i;
+                dist_found = distance;
+                first = 0;
+            };
+            if (distance < dist_found) {
+                if (which != 0) {
+                    bool good = true;
+                    for (int j = 0; j < which; j++) {
+                        {
+                            double X = chambers[which]->points[i][0];
+                            double Y = chambers[which]->points[i][1];
+                            double Z = chambers[which]->points[i][2];
+
+                            double srX0 = chambers[j]->centerX;
+                            double srY0 = chambers[j]->centerY;
+                            double srZ0 = chambers[j]->centerZ;
+
+                            double a2 = (kx_ * chambers[j]->radius) * (kx_ * chambers[j]->radius);
+                            double b2 = (ky_ * chambers[j]->radius) * (ky_ * chambers[j]->radius);
+                            double c2 = (kz_ * chambers[j]->radius) * (kz_ * chambers[j]->radius);
+
+                            double up1 = (X - srX0) * (X - srX0);
+                            double up2 = (Y - srY0) * (Y - srY0);
+                            double up3 = (Z - srZ0) * (Z - srZ0);
+
+                            double exp1 = up1 / a2;
+                            double exp2 = up2 / b2;
+                            double exp3 = up3 / c2;
+
+                            double result = exp1 + exp2 + exp3;
+                            if (result < 1.0) 
+                            {
+                                good = false;
+                            }
+                        }
+                    }
+                    if (good) {
+                        found = i;
+                        dist_found = distance;
+                    }
+                }
+            };
+        };
+    };
+
+    return (found); 
+}
