package JSciDraw.physics;

import JSci.maths.NumericalConstants;
import JSci.maths.AbstractDoubleVector;
import JSci.maths.Double2Vector;
import JSci.physics.ClassicalParticle;
/**
* The RigidBody2D class provides an object for
* encapsulating rigid bodies that live in 2D.
* @version 1.0
* @author Mark Hale
*/
public class RigidBodyV2D extends ClassicalParticle {
        /**
        * Moment of inertia.
        */
        protected double angMass;
        /**
        * Angle (orientation).
        */
        protected double ang;
        /**
        * Angular velocity.
        */
        protected double angVel;
        /**
        * Constructs a rigid body.
        */
        public RigidBodyV2D() {
            super(2);
        }
        /**
        * Sets the moment of inertia.
        */
        public void setMomentOfInertia(double MoI) {
                angMass=MoI;
        }
        /**
        * Returns the moment of inertia.
        */
        public double getMomentOfInertia() {
                return angMass;
        }
        /**
        * Sets the angle (orientation) of this body.
        * @param angle an angle in radians.
        */
        public void setAngle(double angle) {
                ang=angle;
        }
        /**
        * Returns the angle (orientation) of this body.
        * @return an angle in radians.
        */
        public double getAngle() {
                return ang;
        }
        /**
        * Sets the angular velocity.
        */
        public void setAngularVelocity(double angleVel) {
                angVel=angleVel;
        }
        /**
        * Returns the angular velocity.
        */
        public double getAngularVelocity() {
                return angVel;
        }
        public void setAngularMomentum(double angleMom) {
                angVel=angleMom/angMass;
        }
        public double getAngularMomentum() {
                return angMass*angVel;
        }
        /**
        * Returns the kinetic and rotational energy.
        */
        public double energy() {
                double vx = v.getComponent(0); 
                double vy = v.getComponent(1); 
                return (mass*(vx*vx+vy*vy)+angMass*angVel*angVel)/2.0;
        }
        /**
        * Evolves this particle forward according to its kinematics.
        * This method changes the particle's position and orientation.
        * @return this.
        */
        public ClassicalParticle move(double dt) {
              return rotate(dt).translate(dt);
        }
        public ClassicalParticle translate(double dt) {
              x = x.add(v.scalarMultiply(dt));
              return this;
        }
        /**
        * Evolves this particle forward according to its rotational kinematics.
        * This method changes the particle's orientation.
        * @return this.
        */
        public RigidBodyV2D rotate(double dt) {
                ang+=angVel*dt;
                if(ang>NumericalConstants.TWO_PI)
                        ang-=NumericalConstants.TWO_PI;
                else if(ang<0.0)
                        ang+=NumericalConstants.TWO_PI;
                return this;
        }
        /**
        * Accelerates this particle.
        * This method changes the particle's angular velocity.
        * It is additive, that is <code>angularAccelerate(a1, dt).angularAccelerate(a2, dt)</code>
        * is equivalent to <code>angularAccelerate(a1+a2, dt)</code>.
        * @return this.
        */
        public RigidBodyV2D angularAccelerate(double a, double dt) {
                angVel += a*dt;
                return this;
        }
        /**
        * Applies a torque to this particle.
        * This method changes the particle's angular velocity.
        * It is additive, that is <code>applyTorque(T1, dt).applyTorque(T2, dt)</code>
        * is equivalent to <code>applyTorque(T1+T2, dt)</code>.
        * @return this.
        */
        public RigidBodyV2D applyTorque(double T, double dt) {
                return angularAccelerate(T/angMass, dt);
        }
        /**
        * Applies a force acting at a point away from the centre of mass.
        * Any resultant torques are also applied.
        * This method changes the particle's angular velocity.
        * @param x x-coordinate from centre of mass.
        * @param y y-coordinate from centre of mass.
        * @return this.
        */
        public RigidBodyV2D applyForce(AbstractDoubleVector f,AbstractDoubleVector p, double dt) {
                double px = p.getComponent(0);
                double py = p.getComponent(1); 
                double fx = f.getComponent(0);
                double fy = f.getComponent(1); 
                applyTorque(px*fy-py*fx, dt); // T = r x F
                final double k=(px*fx+py*fy)/(px*px+py*py); // r.F/|r|^2
                applyForce(f.scalarMultiply(k), dt);
                return this;
        }
        /**
        * Collides this particle with another.
        * This method calculates the resultant velocities.
        * @param theta centre of mass deflection angle.
        * @param e coefficient of restitution.
        * @return this.
        */
        /*
        public RigidBodyV2D collide(RigidBodyV2D p,Double2Vector c,double e) {
                final double totalMass = mass+p.mass;
                final double v1x =v.getComponent(0);
                final double v1y =v.getComponent(1); 
                final double v2x =p.v.getComponent(0);
                final double v2y =p.v.getComponent(1);                 
                
           
                final double v1px = v1x+angVel* ( c.subtract(x).getComponent(1) ) ;
                final double v1py = v1y-angVel* ( c.subtract(x).getComponent(0) );
                final double v2px = v2x+p.angVel* ( c.subtract(p.x).getComponent(1) ) ;
                final double v2py = v2y-p.angVel* ( c.subtract(p.x).getComponent(0) );

                final AbstractDoubleVector v1p = new Double2Vector(v1px,v1py);
                final AbstractDoubleVector v2p = new Double2Vector(v2px,v2py);
                final AbstractDoubleVector v12 = v1p.subtract(v2p); 
                final AbstractDoubleVector n1 = v1p.scalarMultiply( mass );//xnew Double2Vector( v1x/v.norm(),v1y/v.norm() );
                final AbstractDoubleVector n2 = v2p.scalarMultiply( p.mass ); //new Double2Vector( v2x/p.v.norm(),v2y/p.v.norm() );
                final AbstractDoubleVector n = n1.add(n2); n.normalize();
                //final AbstractDoubleVector f1 = n.scalarMultiply(v2p.norm()); 
                P.r(""+n.scalarProduct(v12) );
                if( n.scalarProduct(v12) > 0)
                      return this;
                 
                final double meanMass = (mass * p.mass)/(mass + p.mass);
                final double r1p = vectorProduct(x,c);// v2y-v1y*v2x;
                final double r2p = vectorProduct(p.x,c);
                final double j = -(1+e)*n.scalarProduct(v12)/(1/meanMass + r1p*r1p/angMass + r2p*r2p/p.angMass);
                P.r(" j " + j +" v" +v + "p.v" +p.v +"r1p" +r1p + "r2p" +r2p);
                setVelocity( v.add(n.scalarMultiply(j/mass )) );
                p.setVelocity( p.v.subtract(n.scalarMultiply(j/p.mass )) );
                angVel = angVel+j*r1p/angMass;
                p.angVel = p.angVel-j*r2p/p.angMass;
                return this;
        }
        private double vectorProduct(AbstractDoubleVector v1,AbstractDoubleVector v2) {
            return v1.getComponent(0)*v2.getComponent(1) - v1.getComponent(1)*v2.getComponent(0);
        }
         **/
        /**
        * Collides this particle with another.
        * This method calculates the resultant velocities.
        * @param theta centre of mass deflection angle.
        * @param e coefficient of restitution.
        * @return this.
        */
        public RigidBodyV2D collide(RigidBodyV2D p,double theta,double e) {
                final double totalMass = mass+p.mass;
                final double deltaVx = p.v.getComponent(0)-v.getComponent(0);
                final double deltaVy = p.v.getComponent(1)-v.getComponent(1);
                final double cos = Math.cos(theta);
                final double sin = Math.sin(theta);
                AbstractDoubleVector tv = new Double2Vector(
                    p.mass*(e*(deltaVx*cos+deltaVy*sin)+deltaVx)/totalMass,
                    p.mass*(e*(deltaVy*cos-deltaVx*sin)+deltaVy)/totalMass
                );
                AbstractDoubleVector tvp = new Double2Vector(
                    mass*(e*(deltaVx*cos+deltaVy*sin)+deltaVx)/totalMass,
                    mass*(e*(deltaVy*cos-deltaVx*sin)+deltaVy)/totalMass
                );
                v = v.add(tv);
                p.v = p.v.subtract(tvp);
                
                //setVelocity()
                /*
                vx += p.mass*(e*(deltaVx*cos+deltaVy*sin)+deltaVx)/totalMass;
                vy += p.mass*(e*(deltaVy*cos-deltaVx*sin)+deltaVy)/totalMass;
                p.vx -= mass*(e*(deltaVx*cos+deltaVy*sin)+deltaVx)/totalMass;
                p.vy -= mass*(e*(deltaVy*cos-deltaVx*sin)+deltaVy)/totalMass;
                 */
                return this;
        }
        
        /**
        * Collides this particle with another.
        * This method calculates the resultant angular velocities.
        * @param e coefficient of restitution.
        * @return this.
        */
        public RigidBodyV2D angularCollide(RigidBodyV2D p,double e) {
                final double meanMass = (angMass+p.angMass)/(e+1.0);
                final double delta = p.angVel-angVel;
                angVel += p.angMass*delta/meanMass;
                p.angVel -= angMass*delta/meanMass;
                return this;
        }  
        
}

