@Override protected void ComputeUntypedRate(IContinuousState ROstate, IContinuousRate WOrate) { SailBoatState etat = (SailBoatState) ROstate; SailBoatRate taux = (SailBoatRate) WOrate; BateauAVoileInit init = (BateauAVoileInit) getInitParameters(); BateauAVoileFeatures tno = (BateauAVoileFeatures) getFeatures(); // if(CurrentDate().toString().compareTo("01/09/2014 06:00:00.62")==0) // System.out.println("ici"); // identification des axes du bateau Rotation rz = new Rotation(Vector3D.PLUS_K, etat.getTheta()); Vector3D axe_x = rz.applyTo(Vector3D.PLUS_I); Vector3D axe_y_horizontal = rz.applyTo(Vector3D.PLUS_J); Rotation r_axe_x = new Rotation(axe_x, etat.getPhi()); Vector3D axe_y = r_axe_x.applyTo(axe_y_horizontal); Vector3D axe_z = r_axe_x.applyTo(Vector3D.PLUS_K); // Points application des forces Vector3D CcG = axe_y_horizontal.scalarMultiply(tno.getCg_CC_Max() * Math.sin(etat.getPhi())); Vector3D CcVc = axe_x.scalarMultiply(tno.getrV()); Vector3D CcRg = axe_x.scalarMultiply(-tno.getrG()); Vector3D CcCV = CcVc.add(voile.getCV()); // bilan des forces Vector3D poids = Environment.g.scalarMultiply(tno.getM()); Vector3D fv = voile.getFv(); Vector3D fg = gouvernail.getFg(); Vector3D f_friction = etat.getVel().scalarMultiply(-tno.getAlphaF()); Vector3D fAntiDerive = axe_y_horizontal.scalarMultiply( -Math.signum(etat.getVel().dotProduct(axe_y_horizontal)) * 0.5 * 1000 * (0.4 * Math.pow(etat.getVel().dotProduct(axe_y_horizontal) * 1.5, 2))); // archimede compense les composantes selon z du gouvernail et de la voile Vector3D archimede = poids .scalarMultiply(-1) .add(Vector3D.PLUS_K.scalarMultiply(-Vector3D.PLUS_K.dotProduct(fg.add(fv)))); forces.clear(); // forces.add(new Force(CcG,poids)); // forces.add(new Force(Vector3D.ZERO,archimede)); forces.add(new Force(CcCV, fv)); forces.add(new Force(CcRg, fg)); forces.add(new Force(CcG, etat.getVel())); // bilan des moments en Cc suivant l'axe x du bateau // contribution de la force vélique M1 = axe_z .scalarMultiply(Math.abs(CcCV.dotProduct(axe_z))) .crossProduct(axe_y.scalarMultiply(fv.dotProduct(axe_y))) .dotProduct(axe_x); // contribution du poids M2 = CcG.crossProduct(poids).dotProduct(axe_x); // antifriction selon M3 = -tno.getAlphaTheta() * etat.getPhi_point(); // bilan des moments en Cc suivant l'axe z du bateau // contribution de la force vélique M4 = CcVc.crossProduct(axe_y.scalarMultiply(fv.dotProduct(axe_y))).dotProduct(axe_z); // contribution du gouvernail M5 = CcRg.crossProduct(axe_y.scalarMultiply(fg.dotProduct(axe_y))).dotProduct(axe_z); // contribution du couple anti friction M6 = -tno.getAlphaTheta() * etat.getTheta_point(); taux.setCurvRate(etat.getVel().getNorm()); // abscisse curviligne taux.setPosRate(etat.getVel()); taux.setVelRate( poids .add(archimede) .add(fv) .add(fg) .add(f_friction) .scalarMultiply(1 / tno.getM()) .add(fAntiDerive)); taux.setPhi_rate(etat.getPhi_point()); taux.setPhi_point_rate((M1 + M2 + M3) * 1.0 / tno.getJx()); taux.setTheta_rate(etat.getTheta_point()); taux.setTheta_point_rate((M4 + M5 + M6) * 1.0 / tno.getJz()); }
@Override public double getPositionRelativeGouvernail() { BateauAVoileFeatures bavf = (BateauAVoileFeatures) getFeatures(); return bavf.getrG(); }