
/**
 * Spring - DESCRIPTION
 * <p/>
 * An extension to the Processing project - http://Proce55ing.net
 * <p/>
 * Copyright (c) 2004 Simon Greenwold
 * Created: Feb 11, 2004
 * <p/>
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * <p/>
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 * <p/>
 * You should have received a copy of the GNU Lesser General
 * Public License along with the Processing project; if not,
 * write to the Free Software Foundation, Inc., 59 Temple Place,
 * Suite 330, Boston, MA  02111-1307  USA
 */

public class Spring extends Force {

    Particle a;
    Particle b;
    private PSystem pSystem;

    float restLength;  // Rest length
    float strength; // Spring constant
    float damping; // Damping constant
    float dist;
    float Fx;
    float Fy;
    float Fz;
    float Lx;
    float Ly;
    float Lz;
    double dx[] = new double[3];
    double nx[] = new double[3];
    
    void draw() {
        if (pSystem == null) return;
        pSystem.getBApplet().line(a.pos[0], a.pos[1], a.pos[2], b.pos[0], b.pos[1], b.pos[2]);
    }

    Spring(Particle a, Particle b) {
        this();
        this.a = a;
        this.b = b;
    }

    Spring() {
    }

    void init(PSystem pSystem) {
        this.pSystem = pSystem;
        strength = pSystem.defaultSpringStrength;
        damping = pSystem.defaultSpringDamping;
        restLength = pSystem.defaultSpringRestLength;
    }

    void applyForce() {
        dist = a.dist(a.pos, b.pos);
        if (dist == 0) {
            return;
        }
        Lx = a.pos[0] - b.pos[0];
        Ly = a.pos[1] - b.pos[1];
        Lz = a.pos[2] - b.pos[2];

        Fx = - (strength * (dist - restLength) + damping * (a.velocity[0] - b.velocity[0]) * Lx / dist) * Lx / dist;
        Fy = - (strength * (dist - restLength) + damping * (a.velocity[1] - b.velocity[1]) * Ly / dist) * Ly / dist;
        Fz = - (strength * (dist - restLength) + damping * (a.velocity[2] - b.velocity[2]) * Lz / dist) * Lz / dist;

        a.addForce(Fx, Fy, Fz);
        b.addForce(-Fx, -Fy, -Fz);
    }

    int timeCounter = 0;

    void applyForce(Particle p) {
        if (a != p && b != p) return;

        dist = a.dist(a.pos, b.pos);
        if (dist == 0) {
            return;
        }
        Lx = a.pos[0] - b.pos[0];
        Ly = a.pos[1] - b.pos[1];
        Lz = a.pos[2] - b.pos[2];
        Fx = - (strength * (dist - restLength) + damping * (a.velocity[0] - b.velocity[0]) * Lx / dist) * Lx / dist;
        Fy = - (strength * (dist - restLength) + damping * (a.velocity[1] - b.velocity[1]) * Ly / dist) * Ly / dist;
        Fz = - (strength * (dist - restLength) + damping * (a.velocity[2] - b.velocity[2]) * Lz / dist) * Lz / dist;

        /*if (timeCounter > 1010) {
        System.out.println("Report particle " + timeCounter + " " + this);
        System.out.println("  " + Fx + " " + Fy + " " + Fz + " " + dist);
        System.out.println("  " + Lx + " " + Ly + " " + Lz);
        System.out.println("  " + strength + " " + restLength + " " + damping);
        System.out.println("  " + a + " " + a.velocity[0] + " " + a.velocity[1] + " " + a.velocity[2]);
        System.out.println("  " + b + " " + b.velocity[0] + " " + b.velocity[1] + " " + b.velocity[2]);
        }  */

        timeCounter++;

        if (a == p)
            a.addForce(Fx, Fy, Fz);
        else
            b.addForce(-Fx, -Fy, -Fz);
    }

public void resetCalcs() {
	// TODO Auto-generated method stub
	dx[0] = a.pos[0] - b.pos[0];
	dx[1] = a.pos[1] - b.pos[1];
	dx[2] = a.pos[2] - b.pos[2];
    dist = (float)java.lang.Math.sqrt( dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2] );
	if (dist != 0) {
		nx[0] = dx[0] / dist;
		nx[1] = dx[1] / dist;
		nx[2] = dx[2] / dist;
	}
	else {
		nx[0] = 0;
		nx[1] = 0;
		nx[2] = 0;
	}
}


/* (non-Javadoc)
 * @see simong.particles.Force#calculateForce()
 */
public void calculateForce(double F0[]) {
	// TODO Auto-generated method stub
	if (dist == 0) {
		zero(Jx);
		zero(Jv);
		zero(B);
	}
	else {
		F[0] = - (strength * (dist - restLength) + damping * (a.velocity[0] - b.velocity[0]) * nx[0]) * nx[0];
		F[1] = - (strength * (dist - restLength) + damping * (a.velocity[1] - b.velocity[1]) * nx[1]) * nx[1];
		F[2] = - (strength * (dist - restLength) + damping * (a.velocity[2] - b.velocity[2]) * nx[2]) * nx[2];
	
		int ai = a.index * 3;
		int bi = b.index * 3;
		F0[ai  ] += F[0];
		F0[ai+1] += F[1];
		F0[ai+2] += F[2];
	
		F0[bi  ] -= F[0];
		F0[bi+1] -= F[1];
		F0[bi+2] -= F[2];
	}
}


/* (non-Javadoc)
 * @see simong.particles.Force#calculateJacobians()
 *  h = timeStep
 */
public void calculateJacobians(double h) {
	// TODO Auto-generated method stub
	/* We calculate spring  Jx = |  ?  ?  ? |
	 							 |  ?  ?  ? |
                                 |  ?  ?  ? |
    */
	
	if (dist == 0) return;
	
	// begin with outer product of nx.
	Jx[0][0] = nx[0] * nx[0]; Jx[0][1] = nx[0] * nx[1]; Jx[0][2] = nx[0] * nx[2];
	Jx[1][0] = nx[1] * nx[0]; Jx[1][1] = nx[1] * nx[1]; Jx[1][2] = nx[1] * nx[2];
	Jx[2][0] = nx[2] * nx[0]; Jx[2][1] = nx[2] * nx[1]; Jx[2][2] = nx[2] * nx[2];
	
	
	//Matrix dxtdx = new DenseMatrix(3,3);
	//dxtdx.rank1(dx); // Outer product dx*dt
	//double len = dx.norm(Vector.Norm.Two);
	//if (len != 0.0) {
	//len = 1.0/len;
	//}
	//dxtdx.scale(len*len);
	
	double len;
	if (dist != 0.0) { 
		len = 1.0 / dist;
	}
	else {
		len = 0;
	}
	
	// Jx = (nxtnx + (I - nxtnx)*(1 - R / L) ) * ks
	double mul = restLength*len - 1;
	Jx[0][0] = (Jx[0][0] + (Jx[0][0]-1)*mul)*strength;   Jx[0][1] = (Jx[0][1] + Jx[0][1]*mul)*strength;     Jx[0][2] = (Jx[0][2] + Jx[0][2]*mul)*strength;
	Jx[1][0] = (Jx[1][0] + Jx[1][0]*mul)*strength;       Jx[1][1] = (Jx[1][1] + (Jx[1][1]-1)*mul)*strength; Jx[1][2] = (Jx[1][2] + Jx[1][2]*mul)*strength;
	Jx[2][0] = (Jx[2][0] + Jx[2][0]*mul)*strength;       Jx[2][1] = (Jx[2][1] + Jx[2][1]*mul)*strength;     Jx[2][2] = (Jx[2][2] + (Jx[2][2]-1)*mul)*strength;	
	
	//Jx.addDiagonal(1.0);
	//Jx.add(-1.0, dxtdx);
	//Jx.scale(1 - restLength*len);
	
	//Jx.add(dxtdx);
	//Jx.scale(strength);
	
	Jv[0][0] = damping;
	Jv[1][1] = damping;
	Jv[2][2] = damping;
	
	//	B = - h * Jx - h^2 * Jv  (cache result used in conj. gad.)
	double hsq = h * h;
	B[0][0] = -h * Jx[0][0] - hsq * Jv[0][0];  B[0][1] = -h * Jx[0][1];                   B[0][2] = -h * Jx[0][2];
	B[1][0] = -h * Jx[1][0];                   B[1][1] = -h * Jx[1][1] - hsq * Jv[1][1];  B[1][2] = -h * Jx[1][2];
	B[2][0] = -h * Jx[2][0];                   B[2][1] = -h * Jx[2][1];                   B[2][2] = -h * Jx[2][2] - hsq * Jv[2][2];
}

}
