public GpsDevice() {
m_latitude = new Measurement(java.lang.Math.toRadians(0), Unit.rad);
m_longitude = new Measurement(java.lang.Math.toRadians(0), Unit.rad);
m_altitude = new Measurement(0, Unit.m);
m_speed = new Measurement(0, Unit.m_s);
m_track = new Measurement(java.lang.Math.toRadians(0), Unit.rad);
}
@Override
public void update(GameContainer gc, StateBasedGame sb, int delta) {
float rotation = owner.getRotation();
float scale = owner.getScale();
Vector2f position = owner.getPosition();
Input input = gc.getInput();
if (input.isKeyDown(Input.KEY_LEFT)) {
rotation += -0.2f * delta;
}
if (input.isKeyDown(Input.KEY_RIGHT)) {
rotation += 0.2f * delta;
}
if (input.isKeyDown(Input.KEY_UP)) {
float hip = 0.45f * delta;
position.x += hip * java.lang.Math.sin(java.lang.Math.toRadians(rotation));
position.y -= hip * java.lang.Math.cos(java.lang.Math.toRadians(rotation));
}
owner.setPosition(position);
owner.setRotation(rotation);
owner.setScale(scale);
}
/// @pre temps > 0
/// @post Retorna una taula(t). t[0] i t[1] són les coordenades de n després de que hagui passat
// temps iteracions
double[] preveurePosicio(Nau n, double temps) {
double dx = temps * n.velocitat_ * Math.cos(Math.toRadians(n.angleVelocitat_));
double dy = temps * n.velocitat_ * -Math.sin(Math.toRadians(n.angleVelocitat_));
double[] t = n.obtenirCentreTriangle();
double centrex = t[0];
double centrey = t[1];
double[] previsio = {centrex + dx, centrey + dy};
return previsio;
}
public void calculateClvsAlpha() {
int nValue = 50;
double alphaMin = Math.toRadians(-5);
double alphaMax = Math.toRadians(25);
alphaArray.linspace(alphaMin, alphaMax, nValue);
double alpha;
double q = _clStar - _clAlpha.getEstimatedValue() * _alphaStar.getEstimatedValue();
clAirfoil = new double[nValue];
for (int i = 0; i < nValue; i++) {
alpha = alphaArray.get(i);
if (alpha < _alphaStar.getEstimatedValue()) {
clAirfoil[i] = _clAlpha.getEstimatedValue() * alpha + q;
} else {
double[][] matrixData = {
{
Math.pow(_alphaStall.getEstimatedValue(), 3),
Math.pow(_alphaStall.getEstimatedValue(), 2),
_alphaStall.getEstimatedValue(),
1.0
},
{
3 * Math.pow(_alphaStall.getEstimatedValue(), 2),
2 * _alphaStall.getEstimatedValue(),
1.0,
0.0
},
{
3 * Math.pow(_alphaStar.getEstimatedValue(), 2),
2 * _alphaStar.getEstimatedValue(),
1.0,
0.0
},
{
Math.pow(_alphaStar.getEstimatedValue(), 3),
Math.pow(_alphaStar.getEstimatedValue(), 2),
_alphaStar.getEstimatedValue(),
1.0
}
};
RealMatrix m = MatrixUtils.createRealMatrix(matrixData);
double[] vector = {_clMax, 0, _clAlpha.getEstimatedValue(), _clStar};
double[] solSystem = MyMathUtils.solveLinearSystem(m, vector);
double a = solSystem[0];
double b = solSystem[1];
double c = solSystem[2];
double d = solSystem[3];
clAirfoil[i] = a * Math.pow(alpha, 3) + b * Math.pow(alpha, 2) + c * alpha + d;
}
}
}
/**
* When scanning a robot we need to add it to the collection of scanned objects so it can be used
* later for updates to the bots movement.
*/
public void onScannedRobot(ScannedRobotEvent e) {
double targetBearing = getHeading() + e.getBearing();
double tmpX = getX() + e.getDistance() * Math.sin(Math.toRadians(targetBearing));
double tmpY = getY() + e.getDistance() * Math.cos(Math.toRadians(targetBearing));
String name = e.getName();
if (name.equals(GOAL_NAME)) {
foundGoal = true;
}
obstacles.put(name, new Enemy(tmpX, tmpY, e.getBearing()));
setTurnRadarRight(getRadarTurnRemaining());
}
/*
* requires a magnitude between -1 and 1 inclusive:
* assumes that the angle is in degrees
* calculates and sets the motor speeds for a given polar vector
* allows for rotation while driving [-1,1]
*/
private void driveMecanum() {
/*
* Does the computation of each motor speed value ever exceed the
* range of -1.0, 1.0? Just curious.
*
* - Mr. Ward
*/
// RRLogger.logDebug(this.getClass(),"driveMecanum()", "driveMecanum()" );
frontLeftMotor.set(-(l_magnitude + rotation) * Math.cos(Math.toRadians((l_angle + 45))));
frontRightMotor.set((l_magnitude - rotation) * Math.sin(Math.toRadians(l_angle + 45)));
backLeftMotor.set(-(l_magnitude + rotation) * Math.sin(Math.toRadians(l_angle + 45)));
backRightMotor.set((l_magnitude - rotation) * Math.cos(Math.toRadians(l_angle + 45)));
}
protected void makeSheet() {
if (false) { // TODO deal with sheets
// sprite sheet is East 0, clockwise
// direction sheet is North 0, clockwise
/*int sheetIndex = (dir.ordinal() - Direction.EAST.ordinal() + 8) % 8;
int soldierHeight = image.getHeight();
if (!isAttacking()) {
sheetIndex += 8;
}
image = image.getSubimage(sheetIndex * soldierHeight, 0,
soldierHeight, soldierHeight);
*/
} else {
sprites = new BufferedImage[8];
sprites[0] = image;
for (int i = 1; i < 8; i++) {
sprites[i] =
new BufferedImage(image.getWidth(), image.getHeight(), BufferedImage.TYPE_INT_ARGB);
double rotationRequired = Math.toRadians(i * 45);
double locationX = image.getWidth() / 2;
double locationY = image.getHeight() / 2;
AffineTransform tx =
AffineTransform.getRotateInstance(rotationRequired, locationX, locationY);
AffineTransformOp op = new AffineTransformOp(tx, AffineTransformOp.TYPE_BILINEAR);
// Drawing the rotated image at the required drawing locations
sprites[i].createGraphics().drawImage(op.filter(image, null), 0, 0, null);
}
}
}
/* Converts an angle measured in degrees to an approximately equivalent angle measured in radians.
*/
public static SchemaTypeNumber radians(SchemaTypeNumber value) {
switch (value.numericType()) {
case SchemaTypeNumber.NUMERIC_VALUE_INT:
return new SchemaInt((int) java.lang.Math.toRadians(value.doubleValue()));
case SchemaTypeNumber.NUMERIC_VALUE_LONG:
return new SchemaLong((long) java.lang.Math.toRadians(value.doubleValue()));
case SchemaTypeNumber.NUMERIC_VALUE_BIGINTEGER:
return new SchemaInteger(
(long)
java.lang.Math.toRadians(value.doubleValue())); // note: possible loss of precision
case SchemaTypeNumber.NUMERIC_VALUE_FLOAT:
return new SchemaFloat((float) java.lang.Math.toRadians(value.doubleValue()));
case SchemaTypeNumber.NUMERIC_VALUE_DOUBLE:
return new SchemaDouble(java.lang.Math.toRadians(value.doubleValue()));
}
return new SchemaDecimal(java.lang.Math.toRadians(value.doubleValue()));
}
private void parseNmeaSentence(String scannedInput) {
double lon, lat, speed, alt, track;
// got a message... do a cksum
if (!NmeaCksum(scannedInput)) {
s_logger.error("NMEA checksum not valid");
return;
}
// s_logger.info(scannedInput);
m_lastSentence = scannedInput;
NMEAParser gpsParser = new NMEAParser();
gpsParser.parseSentence(scannedInput);
m_validPosition = gpsParser.is_validPosition();
// s_logger.debug("Parse : "+scannedInput+" position valid = "+m_validPosition);
if (!m_validPosition) return;
if (!scannedInput.startsWith("$G")) {
// Invalid NMEA String. Return.
s_logger.warn("Invalid NMEA sentence: " + scannedInput);
return;
}
// Remove the first 3 characters from the input string in order to normalize the commands
scannedInput = scannedInput.substring(3);
if (scannedInput.startsWith("TXT")) {
s_logger.debug("U-Blox init message: " + scannedInput);
} else if (scannedInput.startsWith("GGA")) {
try {
lon = gpsParser.get_longNmea();
lat = gpsParser.get_latNmea();
alt = gpsParser.get_altNmea();
m_fixQuality = gpsParser.get_fixQuality();
m_latitude = new Measurement(java.lang.Math.toRadians(lat), Unit.rad);
m_longitude = new Measurement(java.lang.Math.toRadians(lon), Unit.rad);
m_altitude = new Measurement(alt, Unit.m);
m_latitudeNmea = lat;
m_longitudeNmea = lon;
m_altitudeNmea = alt;
m_DOP = gpsParser.get_DOPNmea();
m_nrSatellites = gpsParser.get_nrSatellites();
m_timeNmea = gpsParser.get_timeNmea();
} catch (Exception e) {
m_latitude = null;
m_longitude = null;
m_altitude = null;
m_latitudeNmea = 0;
m_longitudeNmea = 0;
m_altitudeNmea = 0;
}
} else if (scannedInput.startsWith("GLL")) {
try {
lon = gpsParser.get_longNmea();
lat = gpsParser.get_latNmea();
m_latitude = new Measurement(java.lang.Math.toRadians(lat), Unit.rad);
m_longitude = new Measurement(java.lang.Math.toRadians(lon), Unit.rad);
m_latitudeNmea = lat;
m_longitudeNmea = lon;
} catch (Exception e) {
m_latitude = null;
m_longitude = null;
m_latitudeNmea = 0;
m_longitudeNmea = 0;
}
} else if (scannedInput.startsWith("GSA")) {
try {
m_PDOP = gpsParser.get_PDOPNmea();
m_HDOP = gpsParser.get_HDOPNmea();
m_VDOP = gpsParser.get_VDOPNmea();
m_3Dfix = gpsParser.get_3DfixNmea();
// System.out.println("m_PDOP = "+m_PDOP+" m_HDOP = "+m_HDOP+" m_VDOP = "+m_VDOP+"
// m_3Dfix = "+m_3Dfix);
} catch (Exception e) {
m_PDOP = 0;
m_HDOP = 0;
m_VDOP = 0;
m_3Dfix = 0;
}
} else if (scannedInput.startsWith("GSV")) {
} else if (scannedInput.startsWith("RMC")) {
try {
lon = gpsParser.get_longNmea();
lat = gpsParser.get_latNmea();
speed = gpsParser.get_speedNmea();
track = gpsParser.get_trackNmea();
m_latitude = new Measurement(java.lang.Math.toRadians(lat), Unit.rad);
m_longitude = new Measurement(java.lang.Math.toRadians(lon), Unit.rad);
m_speed = new Measurement(speed, Unit.m_s);
m_track = new Measurement(java.lang.Math.toRadians(track), Unit.rad);
m_latitudeNmea = lat;
m_longitudeNmea = lon;
m_speedNmea = speed;
m_trackNmea = track;
m_dateNmea = gpsParser.get_dateNmea();
} catch (Exception e) {
m_latitude = null;
m_longitude = null;
m_speed = null;
m_latitudeNmea = 0;
m_longitudeNmea = 0;
m_speedNmea = 0;
m_trackNmea = 0;
}
} else if (scannedInput.startsWith("VTG")) {
try {
speed = gpsParser.get_speedNmea();
m_speed = new Measurement(speed, Unit.m_s);
m_speedNmea = speed;
} catch (Exception e) {
m_speed = null;
m_speedNmea = 0;
}
} else if (scannedInput.indexOf("FOM") != -1) {
// FOM = scannedInput;
} else if (scannedInput.indexOf("PPS") != -1) {
// PPS = scannedInput;
} else {
s_logger.warn("Unrecognized NMEA sentence: " + scannedInput);
}
}
/** onScannedRobot: What to do when you see another robot */
public void onScannedRobot(ScannedRobotEvent e) {
System.out.println("START at : " + getTime() + " onScannedRobot----------------------------");
String nnn = e.getName();
System.out.println("scan " + nnn);
double eneX =
getX() + Math.sin(e.getBearingRadians() + Math.toRadians(getHeading())) * e.getDistance();
double eneY =
getY() + Math.cos(e.getBearingRadians() + Math.toRadians(getHeading())) * e.getDistance();
Enemy_info enem = null;
if (!isTeammate(nnn)) {
// 味方への情報送信
try {
broadcastMessage(
nnn
+ ", "
+ e.getBearing()
+ ", "
+ e.getBearingRadians()
+ ", "
+ e.getDistance()
+ ", "
+ e.getEnergy()
+ ", "
+ e.getHeading()
+ ", "
+ e.getHeadingRadians()
+ ", "
+ e.getVelocity()
+ ", "
+ eneX
+ ", "
+ eneY);
} catch (IOException ignored) {
}
// スキャンした車両がLocal敵リストにいるかどうかのフラグ
boolean flag = false;
System.out.println("send scanned info");
// スキャンした敵がLocal敵リストの中に存在するか
for (Enemy_info temp : enes) {
if (nnn.equals(temp.get_en_name())) {
flag = true;
enem = temp;
// Local敵リストのアップデート
temp.updateInformation(
e.getBearing(),
e.getBearingRadians(),
e.getDistance(),
e.getEnergy(),
e.getHeading(),
e.getHeadingRadians(),
e.getVelocity(),
eneX,
eneY);
System.out.println(" update scanned Info");
}
}
// スキャンした敵がLocal敵リストの中に存在しない場合
if (!flag) {
// Local敵リストに新規追加
enem =
new Enemy_info(
nnn,
e.getBearing(),
e.getBearingRadians(),
e.getDistance(),
e.getEnergy(),
e.getHeading(),
e.getHeadingRadians(),
e.getVelocity(),
eneX,
eneY);
enes.add(enem);
System.out.println(" add scanned info");
}
if (enemy_detected == false) {
// 共通の敵が設定されていない場合
enemy_detected = true;
target_enemy = enem;
try {
broadcastMessage("Kill , " + target_enemy.get_en_name() + ", !!");
} catch (IOException ignored) {
}
}
if (enemy_detected == true) {
try {
double enemyX = target_enemy.get_en_expX();
double enemyY = target_enemy.get_en_expY();
setTurnRadarLeftRadians(getRadarTurnRemainingRadians());
System.out.println("abs : eX " + enemyX + " : " + "eY " + enemyY);
// 共通の敵が設定されている場合
double enemyBearing = Math.atan((enemyX - getX()) / (enemyY - getY()));
// if(enemyBearing<0){
// System.out.println("change");
// enemyBearing = Math.PI*2 + enemyBearing;
// }else if (enemyBearing < Math.PI){
// }
// System.out.println("atan " + Math.atan((eneY-getY())/(eneX-getX())));
// System.out.println("atan1 " + Math.atan((eneX-getX())/(eneY-getY())));
// System.out.println("trueeee " + (e.getBearingRadians() + this.getHeadingRadians()));
System.out.println(
"enerad"
+ enemyBearing
+ " ?= "
+ "enemyBearing "
+ (this.getHeadingRadians() + e.getBearingRadians()));
System.out.println(enemyBearing + Math.PI);
double enemyHeading = target_enemy.get_en_heading(); // 敵の向き
System.out.println("enemy heading:" + enemyHeading);
// double enemyBearing = this.getHeadingRadians() +
// target_enemy.get_en_bearingRadians();// 自分と敵の角度
// double enemyX = target_enemy.get_en_distance() * Math.sin(enemyBearing);
// double enemyY = target_enemy.get_en_distance() * Math.cos(enemyBearing);
enemyX = enemyX - getX();
enemyY = enemyY - getY();
System.out.println("Relative : eX " + enemyX + " : " + "eY " + enemyY);
double battlefieldWidh = getBattleFieldWidth(); // フィールド幅
double battlefieldHeight = getBattleFieldHeight(); // フィールド高さ
boolean isHeadingToCenter = (int) enemyHeading % 90 == 0; // 中心を向いている
boolean isOnWall =
nearlyEquals(enemyX, 18)
|| nearlyEquals(enemyX + 18, battlefieldWidh)
|| nearlyEquals(enemyY, 18)
|| nearlyEquals(enemyY + 18, battlefieldHeight); // 壁に張り付いている
// 中心を向いている&&壁際にいる(=Walls)なら射撃
if (isHeadingToCenter && isOnWall) {
System.out.println("Walls!!");
}
double dis = 0;
double heading = lastEnemyHeading;
do {
dis += Rules.getBulletSpeed(power);
heading += target_enemy.get_en_headingRadians() - lastEnemyHeading;
enemyX += target_enemy.get_en_velocity() * Math.sin(heading);
enemyY += target_enemy.get_en_velocity() * Math.cos(heading);
} while (dis < Point2D.distance(0, 0, enemyX, enemyY)); //
// 相対角度に変換した上で砲塔の向きを変える
setTurnGunRightRadians(
Utils.normalRelativeAngle(Math.atan2(enemyX, enemyY) - getGunHeadingRadians()));
setFire(power);
// lastEnemyHeading = e.getHeadingRadians();
lastEnemyHeading = target_enemy.get_en_headingRadians();
System.out.println("lastEnemyHeading " + e.getHeadingRadians());
System.out.println(lastEnemyHeading);
// 敵の居る方向へターンする
// setTurnRightRadians(e.getBearingRadians());
setTurnRightRadians(enemyBearing - this.getHeadingRadians());
System.out.println("setTurnRightRadians " + e.getBearingRadians());
System.out.println(enemyBearing - this.getHeadingRadians());
// 前進する
setAhead(moveAmount);
} catch (NullPointerException ee) {
System.out.println("NullPointerException");
System.out.println(target_enemy);
}
}
}
System.out.println("enemy_detected = " + enemy_detected);
System.out.println("target is " + target_enemy.get_en_name());
System.out.println(target_enemy.get_en_expX() + " " + target_enemy.get_en_expY());
System.out.println("END at : " + getTime() + " onScannedRobot----------------------------");
}
public static void main(String[] args) {
double deg = 30;
double rad = Math.toRadians(deg);
double ans = Math.atan(rad);
System.out.println(ans);
}
/*
* Drives the left and right wheels separately, with its repsective xbox stick
* Allows for more precise rotation
*/
private void driveTank() {
frontLeftMotor.set(-(l_magnitude + rotation) * Math.cos(Math.toRadians((l_angle + 45))));
frontRightMotor.set((r_magnitude - rotation) * Math.sin(Math.toRadians(r_angle + 45)));
backLeftMotor.set(-(l_magnitude + rotation) * Math.sin(Math.toRadians(l_angle + 45)));
backRightMotor.set((r_magnitude - rotation) * Math.cos(Math.toRadians(r_angle + 45)));
}
private void initializeAerodynamics(Airfoil airf, AirfoilEnum airfoilName) {
switch (airfoilName) {
case NACA23_018:
_id = airf.getId() + "1" + idCounter + "99";
idCounter++;
airf.setName(AirfoilEnum.NACA23_018);
_theAirfoil = airf;
geometry = airf.getGeometry();
_alphaZeroLift = Amount.valueOf(Math.toRadians(-1.2), SI.RADIAN);
_clAlpha = Amount.valueOf(6.3, SI.RADIAN.inverse());
_alphaStar = Amount.valueOf(Math.toRadians(9.5), SI.RADIAN); // end-of-linearity
_clStar = 1.3;
_alphaStall = Amount.valueOf(Math.toRadians(16.0), SI.RADIAN);
_clMax = 1.65; // 1.8;
_cdMin = 0.00675;
_clAtCdMin = 0.3;
_kFactorDragPolar = 0.004;
_aerodynamicCenterX = 0.243;
_cmAC = -0.02;
_cmACStall = -0.09;
_cmAlphaAC = 0.;
calculateMachCr = new CalculateMachCr();
calculateCdWaveDrag = new CalculateCdWaveDrag();
break;
case NACA23_015:
airf.setName(AirfoilEnum.NACA23_015);
_id = airf.getId() + "1" + idCounter + "99";
idCounter++;
_theAirfoil = airf;
geometry = airf.getGeometry();
_alphaZeroLift = Amount.valueOf(Math.toRadians(-1.1), SI.RADIAN);
_clAlpha = Amount.valueOf(6.3, SI.RADIAN.inverse());
_alphaStar = Amount.valueOf(Math.toRadians(10), SI.RADIAN); // end-of-linearity
_clStar = 1.2;
_alphaStall = Amount.valueOf(Math.toRadians(18.0), SI.RADIAN);
_clMax = 1.7; // 1.6;
_cdMin = 0.00625;
_clAtCdMin = 0.1;
_kFactorDragPolar = 0.004;
_aerodynamicCenterX = 0.243;
_cmAC = -0.02;
_cmACStall = -0.07;
_cmAlphaAC = 0.;
calculateMachCr = new CalculateMachCr();
calculateCdWaveDrag = new CalculateCdWaveDrag();
break;
case NACA23_012:
airf.setName(AirfoilEnum.NACA23_012);
_id = airf.getId() + "1" + idCounter + "99";
idCounter++;
_theAirfoil = airf;
geometry = airf.getGeometry();
_alphaZeroLift = Amount.valueOf(Math.toRadians(-1.32), SI.RADIAN);
_clAlpha = Amount.valueOf(6.3, SI.RADIAN.inverse());
_alphaStar = Amount.valueOf(Math.toRadians(14), SI.RADIAN); // end-of-linearity
_clStar = 1.6;
_alphaStall = Amount.valueOf(Math.toRadians(18), SI.RADIAN);
_clMax = 1.8; // 1.5;
_cdMin = 0.00575;
_clAtCdMin = 0.23;
_kFactorDragPolar = 0.004;
_aerodynamicCenterX = 0.247;
_cmAC = -0.03;
_cmACStall = -0.09;
_cmAlphaAC = 0.;
calculateMachCr = new CalculateMachCr();
calculateCdWaveDrag = new CalculateCdWaveDrag();
break;
case NACA65_209:
_id = airf.getId() + "1" + idCounter + "99";
idCounter++;
_theAirfoil = airf;
geometry = airf.getGeometry();
_alphaZeroLift = Amount.valueOf(Math.toRadians(-1.3), SI.RADIAN);
_clAlpha = Amount.valueOf(5.96, SI.RADIAN.inverse());
_alphaStar = Amount.valueOf(Math.toRadians(11), SI.RADIAN); // end-of-linearity
_clStar = 1.1;
_alphaStall = Amount.valueOf(Math.toRadians(17.0), SI.RADIAN);
_clMax = 1.6;
_cdMin = 0.025;
_clAtCdMin = 0.2;
_kFactorDragPolar = 0.0035;
_aerodynamicCenterX = 0.25;
_cmAC = -0.07;
_cmACStall = -0.09;
_cmAlphaAC = 0.;
calculateMachCr = new CalculateMachCr();
calculateCdWaveDrag = new CalculateCdWaveDrag();
break;
case NACA65_206:
_id = airf.getId() + "1" + idCounter + "99";
idCounter++;
_theAirfoil = airf;
geometry = airf.getGeometry();
_alphaZeroLift = Amount.valueOf(Math.toRadians(-1.4), SI.RADIAN);
_clAlpha = Amount.valueOf(6.13, SI.RADIAN.inverse());
_alphaStar = Amount.valueOf(Math.toRadians(10.0), SI.RADIAN); // end-of-linearity
_clStar = _clAlpha.getEstimatedValue() * _alphaStar.getEstimatedValue();
_alphaStall = Amount.valueOf(Math.toRadians(15.0), SI.RADIAN);
_clMax = 1.3;
_cdMin = 0.025;
_clAtCdMin = 0.2;
_kFactorDragPolar = 0.0035;
_aerodynamicCenterX = 0.25;
_cmAC = -0.07;
_cmACStall = -0.09;
_cmAlphaAC = 0.;
calculateMachCr = new CalculateMachCr();
calculateCdWaveDrag = new CalculateCdWaveDrag();
break;
case NACA0012:
airf.setName(AirfoilEnum.NACA0012);
_id = airf.getId() + "1" + idCounter + "99";
idCounter++;
_theAirfoil = airf;
geometry = airf.getGeometry();
_alphaZeroLift = Amount.valueOf(Math.toRadians(0), SI.RADIAN);
_clAlpha = Amount.valueOf(6.92, SI.RADIAN.inverse());
_alphaStar = Amount.valueOf(Math.toRadians(11), SI.RADIAN); // end-of-linearity
_clStar = 1.23;
_alphaStall = Amount.valueOf(Math.toRadians(20.1), SI.RADIAN);
_clMax = 1.86; // 1.5;
_cdMin = 0.0055;
_clAtCdMin = 0.0;
_kFactorDragPolar = 0.0035;
_aerodynamicCenterX = 0.25;
_cmAC = 0.0;
_cmACStall = -0.09;
_cmAlphaAC = 0.;
calculateMachCr = new CalculateMachCr();
calculateCdWaveDrag = new CalculateCdWaveDrag();
break;
case DFVLR_R4:
airf.setName(AirfoilEnum.DFVLR_R4);
_id = airf.getId() + "1" + idCounter + "99";
idCounter++;
_theAirfoil = airf;
geometry = airf.getGeometry();
_alphaZeroLift = Amount.valueOf(Math.toRadians(-3.75), SI.RADIAN);
_clAlpha = Amount.valueOf(6.3, SI.RADIAN.inverse());
_alphaStar = Amount.valueOf(Math.toRadians(11), SI.RADIAN); // end-of-linearity
_clStar = 1.53;
_alphaStall = Amount.valueOf(Math.toRadians(16.75), SI.RADIAN);
_clMax = 1.7671; // 1.5;
_cdMin = 0.0056;
_clAtCdMin = 0.0;
_kFactorDragPolar = 0.075;
_aerodynamicCenterX = 0.25;
_cmAC = -0.1168;
_cmACStall = -0.0515;
_cmAlphaAC = 0.0015;
calculateMachCr = new CalculateMachCr();
calculateCdWaveDrag = new CalculateCdWaveDrag();
break;
default:
break;
}
}
/**
* Create a unit vector based from the direction of the entity's head.
*
* <p>This equivalent to calling {@link #fromYawPitch(double, double)} using entity's rotations as
* inputs.
*
* @param entity The entity to use
*/
public static Vector fromEntityLook(Entity entity) {
return fromYawPitch(Math.toRadians(entity.rotationYaw), Math.toRadians(entity.rotationPitch));
}
/**
* <strong>Assuming</strong> this vector represents spherical coordinates in degrees, returns a
* new spherical coordinate vector which is in radians.
*/
public Vector toRadians() {
return new Vector(Math.toRadians(x()), Math.toRadians(y()), Math.toRadians(z()));
}
/** @author Mike Evers + Mal */
public class Human extends Character implements Serializable {
private double flashlightRange = 200, flashlightAngle = Math.toRadians(30);
private boolean hasKey;
private double[][] flashlightPoints;
private double flX3 = 0,
flY3 = 0,
flX2 = 0,
flY2 = 0,
flX1,
flY1,
flY23,
flX32,
flY31,
flX13,
flDet,
flMinD,
flMaxD;
/**
* this function sets the variable hasKey
*
* @param hasKey true if the human has the key.
*/
public void setHasKey(boolean hasKey) {
this.hasKey = hasKey;
}
/**
* The Constructor for human. This initializes the flashlightRange, flashlightAngle and the
* flashlightPoints by calling setFlashlight Also initialize the base class Character!
*/
public Human() {
super(null);
this.hasKey = false;
}
/** @return true if human has picked up the key */
public boolean hasKey() {
return hasKey;
}
// <editor-fold defaultstate="collapsed" desc="Flashlight">
/** @return the flashlight range */
public double getFlashlightRange() {
return this.flashlightRange;
}
/**
* Sets the flashlight range
*
* @param flashlightRange
*/
public void setFlashlightRange(int flashlightRange) {
this.flashlightRange = flashlightRange;
}
/** @return the flashlight angle */
public double getFlashlightAngle() {
return this.flashlightAngle;
}
/**
* Sets the flashlight angle
*
* @param flashlightAngle
*/
public void setFlashlightAngle(int flashlightAngle) {
this.flashlightAngle = flashlightAngle;
}
/**
* Initializes the flashlight for the human. With this, the human can see the things in the level,
* but only the things in range of the flashlight. Flashlight is a triangle.
*/
private void setFlashlight() {
flX1 = this.getPosition().getX() + 50;
flY1 = this.getPosition().getY() + 50;
switch (this.getDirection()) {
case UP:
flX2 = flX1 - tan(this.flashlightAngle) * this.flashlightRange;
flY2 = flY1 - this.flashlightRange;
flX3 = flX1 + tan(this.flashlightAngle) * this.flashlightRange;
flY3 = flY1 - this.flashlightRange;
break;
case DOWN:
flX2 = flX1 + tan(this.flashlightAngle) * this.flashlightRange;
flX3 = flX1 - tan(this.flashlightAngle) * this.flashlightRange;
flY2 = flY1 + this.flashlightRange;
flY3 = flY2;
break;
case RIGHT:
flX2 = flX1 + this.flashlightRange;
flX3 = flX2;
flY2 = flY1 + tan(this.flashlightAngle) * this.flashlightRange;
flY3 = flY1 - tan(this.flashlightAngle) * this.flashlightRange;
break;
case LEFT:
flX2 = flX1 - this.flashlightRange;
flX3 = flX2;
flY2 = flY1 + tan(this.flashlightAngle) * this.flashlightRange;
flY3 = flY1 - tan(this.flashlightAngle) * this.flashlightRange;
break;
}
}
/**
* Returns the coordinates of the flashlight polygon So the fx part can draw this for the human.
*
* @return polygon of the flashlight.
*/
public int[] getFlashlightPolygon() {
int[] i = new int[6];
i[0] = (int) this.getPosition().getX() + 50;
i[1] = (int) this.getPosition().getY() + 50;
i[2] = (int) flX2;
i[3] = (int) flY2;
i[4] = (int) flX3;
i[5] = (int) flY3;
return i;
}
// </editor-fold>
/** if haskey == false, hasKey becomes true */
public void pickUpKey() {
if (!hasKey) {
hasKey = true;
}
}
/**
* if haskey == true, enterDoor will cause endround, so the game can continue to the next round or
* go to the victory screen. Otherwise, this method won't cause anything.
*
* @param game
* @throws java.lang.InterruptedException
* @throws java.io.IOException
*/
public void enterDoor(Game game) throws InterruptedException, IOException {
// First check if this entering was on the last floor (last level).
if (game.getFloorAmount() == game.getCurrentFloor()) {
boolean humanFound = false;
while (!humanFound) {
for (IPlayer player : game.getPlayers()) {
try {
if (player.getCharacter() instanceof Human) {
game.endGame();
humanFound = true;
}
} catch (RemoteException ex) {
Logger.getLogger(Human.class.getName()).log(Level.SEVERE, null, ex);
}
}
}
} else {
game.setRoundEnded(true);
}
}
/**
* check if hitbox collides with the points
*
* @param point1
* @param width1
* @param height1
* @param point2
* @param width2
* @param height2
* @return
*/
public boolean checkHitboxCollision(
Point2D point1, int width1, int height1, Point2D point2, int width2, int height2) {
// convert point1 in leftmost and rightmost X value and top and bottom Y value;
int p1Xmin = (int) point1.getX();
int p1Xmax = (int) p1Xmin + width1 - 1;
int p1Ymin = (int) point1.getY();
int p1Ymax = (int) p1Ymin + height1 - 1;
// convert point2 in leftmost and rightmost X value and top and bottom Y value;
int p2Xmin = (int) point2.getX();
int p2Xmax = (int) p2Xmin + width2 - 1;
int p2Ymin = (int) point2.getY();
int p2Ymax = (int) p2Ymin + height2 - 1;
return (betweenInclusive(p1Xmax, p2Xmin, p2Xmax)
&& (betweenInclusive(p1Ymin, p2Ymin, p2Ymax)
|| betweenInclusive(p1Ymax, p2Ymin, p2Ymax)))
|| (betweenInclusive(p1Xmin, p2Xmin, p2Xmax)
&& (betweenInclusive(p1Ymin, p2Ymin, p2Ymax)
|| betweenInclusive(p1Ymax, p2Ymin, p2Ymax)));
}
/**
* check if human collides with a ghost
*
* @param game
* @return the ghost where the human collides with
*/
public Ghost checkGhostCollision(Game game) {
// ghost collision
for (Ghost ghost : game.getGhosts()) {
if (!ghost.getRip() && !ghost.getDead()) {
if (checkHitboxCollision(this.getPosition(), 90, 90, ghost.getPosition(), 90, 90)) {
return ghost;
}
}
}
return null;
}
/**
* check if the given position collides with the flashlight.
*
* @param point
* @return
*/
public boolean flashlightCollision(Point2D point) {
flY23 = flY2 - flY3;
flX32 = flX3 - flX2;
flY31 = flY3 - flY1;
flX13 = flX1 - flX3;
flDet = flY23 * flX13 - flX32 * flY31;
flMinD = Math.min(flDet, 0);
flMaxD = Math.max(flDet, 0);
double x = point.getX();
double y = point.getY();
double dx = x - flX3;
double dy = y - flY3;
double a = flY23 * dx + flX32 * dy;
if (a < flMinD || a > flMaxD) {
return false;
}
double b = flY31 * dx + flX13 * dy;
if (b < flMinD || b > flMaxD) {
return false;
}
double c = flDet - a - b;
return !(c < flMinD || c > flMaxD);
}
/**
* check if the human interacts with ghost, key, door or wall
*
* @param game
* @throws java.lang.InterruptedException
* @throws java.io.IOException
*/
public void checkInteract(Game game) throws InterruptedException, IOException {
DirectionType doorDisplacement = game.getLevel().getDoorDirection();
Point2D door =
new Point2D.Double(
game.getLevel().getDoorLocation().getX(), game.getLevel().getDoorLocation().getY());
switch (doorDisplacement) {
case UP:
door.setLocation(door.getX(), door.getY() - 100);
break;
case RIGHT:
door.setLocation(door.getX() + 100, door.getY());
break;
case LEFT:
door.setLocation(door.getX() - 100, door.getY());
break;
case DOWN:
door.setLocation(door.getX(), door.getY() + 100);
break;
}
Point2D key = game.getLevel().getKeyLocation();
// door collision
if ((checkHitboxCollision(
new Point2D.Double(this.getPosition().getX() + 10, this.getPosition().getY() + 10),
80,
80,
new Point2D.Double(door.getX() - 5, door.getY() - 5),
110,
110)
&& this.hasKey)) // key collision
{
this.enterDoor(game);
}
// key collision
if (checkHitboxCollision(this.getPosition(), 80, 80, key, 80, 80)) {
this.pickUpKey();
}
// flashlight and ghost collision
if (this.checkGhostCollision(game) != null) {
this.checkGhostCollision(game).possess(game);
}
setFlashlight();
game.getGhosts()
.stream()
.forEach(
(g) -> {
if (g.isVulnerable() && !g.getRip() && !g.getDead()) {
boolean hit = false;
for (Point2D p : g.getHitboxPoints()) {
if (flashlightCollision(p)) {
hit = true;
break;
}
}
if (hit) {
g.setTimeOfDeath();
g.vanish(game);
}
}
});
}
}
private static double apparentLongitude(double omicron, double omega, double t) {
return omicron - 0.00569 - 0.00478 * Math.sin(Math.toRadians(omega));
}
/**
* Computes the distance, azimuth, and back azimuth between two lat-lon positions on the Earth's
* surface. Reference ellipsoid is the WGS-84.
*
* <p>You may use a default Earth (equator radius = 6378137.0 meters, flattening = 1.0 /
* 298.257223563) or you may define your own using a ucar.unidata.geoloc.Earth object.
*
* @author Unidata Development Team
*/
public class Bearing {
/** Default Earth. Major radius and flattening; */
private static final Earth defaultEarth = new Earth(6378137.0, 0., 298.257223563);
/** Earth radius */
private static double A;
// private static double A = 6378137.0; // in meters (for reference)
/** The Earth flattening value */
private static double F;
// private static double F = 1.0 / 298.257223563; (for reference)
/** epsilon */
private static final double EPS = 0.5E-13;
/** constant R */
// private static final double R = 1.0 - F; (for reference)
private static double R;
/** conversion for degrees to radians */
private static final double rad = Math.toRadians(1.0);
/** conversion for radians to degrees */
private static final double deg = Math.toDegrees(1.0);
/**
* Calculate the bearing between the 2 points. See calculateBearing below.
*
* @param e Earth object (defines radius & flattening)
* @param pt1 Point 1
* @param pt2 Point 2
* @param result Object to use if non-null
* @return The bearing
*/
public static Bearing calculateBearing(
Earth e, LatLonPoint pt1, LatLonPoint pt2, Bearing result) {
return calculateBearing(
e, pt1.getLatitude(), pt1.getLongitude(), pt2.getLatitude(), pt2.getLongitude(), result);
}
/**
* Calculate the bearing between the 2 points. See calculateBearing below. Uses default Earth
* object.
*
* @param pt1 Point 1
* @param pt2 Point 2
* @param result Object to use if non-null
* @return The bearing
*/
public static Bearing calculateBearing(LatLonPoint pt1, LatLonPoint pt2, Bearing result) {
return calculateBearing(
defaultEarth,
pt1.getLatitude(),
pt1.getLongitude(),
pt2.getLatitude(),
pt2.getLongitude(),
result);
}
/** _more_ */
private static int maxLoopCnt = 0;
/**
* Computes distance (in km), azimuth (degrees clockwise positive from North, 0 to 360), and back
* azimuth (degrees clockwise positive from North, 0 to 360), from latitude-longituide point pt1
* to latitude-longituide pt2. Uses default Earth object.
*
* @param lat1 Lat of point 1
* @param lon1 Lon of point 1
* @param lat2 Lat of point 2
* @param lon2 Lon of point 2
* @param result put result here, or null to allocate
* @return a Bearing object with distance (in km), azimuth from pt1 to pt2 (degrees, 0 = north,
* clockwise positive)
*/
public static Bearing calculateBearing(
double lat1, double lon1, double lat2, double lon2, Bearing result) {
return calculateBearing(defaultEarth, lat1, lon1, lat2, lon2, result);
}
/**
* Computes distance (in km), azimuth (degrees clockwise positive from North, 0 to 360), and back
* azimuth (degrees clockwise positive from North, 0 to 360), from latitude-longituide point pt1
* to latitude-longituide pt2.
*
* <p>Algorithm from U.S. National Geodetic Survey, FORTRAN program "inverse," subroutine
* "INVER1," by L. PFEIFER and JOHN G. GERGEN. See
* http://www.ngs.noaa.gov/TOOLS/Inv_Fwd/Inv_Fwd.html
*
* <p>Original documentation: <br>
* SOLUTION OF THE GEODETIC INVERSE PROBLEM AFTER T.VINCENTY <br>
* MODIFIED RAINSFORD'S METHOD WITH HELMERT'S ELLIPTICAL TERMS <br>
* EFFECTIVE IN ANY AZIMUTH AND AT ANY DISTANCE SHORT OF ANTIPODAL <br>
* STANDPOINT/FOREPOINT MUST NOT BE THE GEOGRAPHIC POLE Reference ellipsoid is the WGS-84
* ellipsoid. <br>
* See http://www.colorado.edu/geography/gcraft/notes/datum/elist.html
*
* <p>Requires close to 1.4 E-5 seconds wall clock time per call on a 550 MHz Pentium with Linux
* 7.2.
*
* @param e Earth object (defines radius and flattening)
* @param lat1 Lat of point 1
* @param lon1 Lon of point 1
* @param lat2 Lat of point 2
* @param lon2 Lon of point 2
* @param result put result here, or null to allocate
* @return a Bearing object with distance (in km), azimuth from pt1 to pt2 (degrees, 0 = north,
* clockwise positive)
*/
public static Bearing calculateBearing(
Earth e, double lat1, double lon1, double lat2, double lon2, Bearing result) {
if (result == null) {
result = new Bearing();
}
if ((lat1 == lat2) && (lon1 == lon2)) {
result.distance = 0;
result.azimuth = 0;
result.backazimuth = 0;
return result;
}
A = e.getMajor();
F = e.getFlattening();
R = 1.0 - F;
// Algorithm from National Geodetic Survey, FORTRAN program "inverse,"
// subroutine "INVER1," by L. PFEIFER and JOHN G. GERGEN.
// http://www.ngs.noaa.gov/TOOLS/Inv_Fwd/Inv_Fwd.html
// Conversion to JAVA from FORTRAN was made with as few changes as possible
// to avoid errors made while recasting form, and to facilitate any future
// comparisons between the original code and the altered version in Java.
// Original documentation:
// SOLUTION OF THE GEODETIC INVERSE PROBLEM AFTER T.VINCENTY
// MODIFIED RAINSFORD'S METHOD WITH HELMERT'S ELLIPTICAL TERMS
// EFFECTIVE IN ANY AZIMUTH AND AT ANY DISTANCE SHORT OF ANTIPODAL
// STANDPOINT/FOREPOINT MUST NOT BE THE GEOGRAPHIC POLE
// A IS THE SEMI-MAJOR AXIS OF THE REFERENCE ELLIPSOID
// F IS THE FLATTENING (NOT RECIPROCAL) OF THE REFERNECE ELLIPSOID
// LATITUDES GLAT1 AND GLAT2
// AND LONGITUDES GLON1 AND GLON2 ARE IN RADIANS POSITIVE NORTH AND EAST
// FORWARD AZIMUTHS AT BOTH POINTS RETURNED IN RADIANS FROM NORTH
//
// Reference ellipsoid is the WGS-84 ellipsoid.
// See http://www.colorado.edu/geography/gcraft/notes/datum/elist.html
// FAZ is forward azimuth in radians from pt1 to pt2;
// BAZ is backward azimuth from point 2 to 1;
// S is distance in meters.
//
// Conversion to JAVA from FORTRAN was made with as few changes as possible
// to avoid errors made while recasting form, and to facilitate any future
// comparisons between the original code and the altered version in Java.
//
// IMPLICIT REAL*8 (A-H,O-Z)
// COMMON/CONST/PI,RAD
// COMMON/ELIPSOID/A,F
double GLAT1 = rad * lat1;
double GLAT2 = rad * lat2;
double TU1 = R * Math.sin(GLAT1) / Math.cos(GLAT1);
double TU2 = R * Math.sin(GLAT2) / Math.cos(GLAT2);
double CU1 = 1. / Math.sqrt(TU1 * TU1 + 1.);
double SU1 = CU1 * TU1;
double CU2 = 1. / Math.sqrt(TU2 * TU2 + 1.);
double S = CU1 * CU2;
double BAZ = S * TU2;
double FAZ = BAZ * TU1;
double GLON1 = rad * lon1;
double GLON2 = rad * lon2;
double X = GLON2 - GLON1;
double D, SX, CX, SY, CY, Y, SA, C2A, CZ, E, C;
int loopCnt = 0;
do {
loopCnt++;
// Check for an infinite loop
if (loopCnt > 1000) {
throw new IllegalArgumentException(
"Too many iterations calculating bearing:"
+ lat1
+ " "
+ lon1
+ " "
+ lat2
+ " "
+ lon2);
}
SX = Math.sin(X);
CX = Math.cos(X);
TU1 = CU2 * SX;
TU2 = BAZ - SU1 * CU2 * CX;
SY = Math.sqrt(TU1 * TU1 + TU2 * TU2);
CY = S * CX + FAZ;
Y = Math.atan2(SY, CY);
SA = S * SX / SY;
C2A = -SA * SA + 1.;
CZ = FAZ + FAZ;
if (C2A > 0.) {
CZ = -CZ / C2A + CY;
}
E = CZ * CZ * 2. - 1.;
C = ((-3. * C2A + 4.) * F + 4.) * C2A * F / 16.;
D = X;
X = ((E * CY * C + CZ) * SY * C + Y) * SA;
X = (1. - C) * X * F + GLON2 - GLON1;
// IF(DABS(D-X).GT.EPS) GO TO 100
} while (Math.abs(D - X) > EPS);
if (loopCnt > maxLoopCnt) {
maxLoopCnt = loopCnt;
// System.err.println("loopCnt:" + loopCnt);
}
FAZ = Math.atan2(TU1, TU2);
BAZ = Math.atan2(CU1 * SX, BAZ * CX - SU1 * CU2) + Math.PI;
X = Math.sqrt((1. / R / R - 1.) * C2A + 1.) + 1.;
X = (X - 2.) / X;
C = 1. - X;
C = (X * X / 4. + 1.) / C;
D = (0.375 * X * X - 1.) * X;
X = E * CY;
S = 1. - E - E;
S = ((((SY * SY * 4. - 3.) * S * CZ * D / 6. - X) * D / 4. + CZ) * SY * D + Y) * C * A * R;
result.distance = S / 1000.0; // meters to km
result.azimuth = FAZ * deg; // radians to degrees
if (result.azimuth < 0.0) {
result.azimuth += 360.0; // reset azs from -180 to 180 to 0 to 360
}
result.backazimuth = BAZ * deg; // radians to degrees; already in 0 to 360 range
return result;
}
/*
* This method is for same use as calculateBearing, but has much simpler calculations
* by assuming a spherical earth. It is actually slower than
* "calculateBearing" code, probably due to having more trig function calls.
* It is less accurate, too.
* Errors are on the order of 1/300 or less. This code
* saved here only as a warning to future programmers thinking of this approach.
*
* Requires close to 2.0 E-5 seconds wall clock time per call
* on a 550 MHz Pentium with Linux 7.2.
*
* public static Bearing calculateBearingAlternate
* (LatLonPoint pt1, LatLonPoint pt2, Bearing result) {
*
* // to convert degrees to radians, multiply by:
* final double rad = Math.toRadians(1.0);
* // to convert radians to degrees:
* final double deg = Math.toDegrees(1.0);
*
* if (result == null)
* result = new Bearing();
*
* double R = 6371008.7; // mean earth radius in meters; WGS 84 definition
* double GLAT1 = rad*(pt1.getLatitude());
* double GLAT2 = rad*(pt2.getLatitude());
* double GLON1 = rad*(pt1.getLongitude());
* double GLON2 = rad*(pt2.getLongitude());
*
* // great circle angular separation in radians
* double alpha = Math.acos( Math.sin(GLAT1)*Math.sin(GLAT2)
* +Math.cos(GLAT1)*Math.cos(GLAT2)*Math.cos(GLON1-GLON2) );
* // great circle distance in meters
* double gcd = R * alpha;
*
* result.distance = gcd / 1000.0; // meters to km
*
* // forward azimuth from point 1 to 2 in radians
* double s2 = rad*(90.0-pt2.getLatitude());
* double FAZ = Math.asin(Math.sin(s2)*Math.sin(GLON2-GLON1) / Math.sin(alpha));
*
* result.azimuth = FAZ * deg; // radians to degrees
* if (result.azimuth < 0.0)
* result.azimuth += 360.0; // reset az from -180 to 180 to 0 to 360
*
* // back azimuth from point 2 to 1 in radians
* double s1 = rad*(90.0-pt1.getLatitude());
* double BAZ = Math.asin(Math.sin(s1)*Math.sin(GLON1-GLON2) / Math.sin(alpha));
*
* result.backazimuth = BAZ * deg;
* if (result.backazimuth < 0.0)
* result.backazimuth += 360.0; // reset backaz from -180 to 180 to 0 to 360
*
* return result;
* }
*/
/** the azimuth, degrees, 0 = north, clockwise positive */
private double azimuth;
/** the back azimuth, degrees, 0 = north, clockwise positive */
private double backazimuth;
/** separation in kilometers */
private double distance;
/**
* Get the azimuth in degrees, 0 = north, clockwise positive
*
* @return azimuth in degrees
*/
public double getAngle() {
return azimuth;
}
/**
* Get the back azimuth in degrees, 0 = north, clockwise positive
*
* @return back azimuth in degrees
*/
public double getBackAzimuth() {
return backazimuth;
}
/**
* Get the distance in kilometers
*
* @return distance in km
*/
public double getDistance() {
return distance;
}
/**
* Nice format.
*
* @return return a nice format of this Bearing
*/
public String toString() {
StringBuilder buf = new StringBuilder();
buf.append("Azimuth: ");
buf.append(azimuth);
buf.append(" Back azimuth: ");
buf.append(backazimuth);
buf.append(" Distance: ");
buf.append(distance);
return buf.toString();
}
/**
* Test the calculations - forward and back
*
* @param args non used
*/
public static void main(String[] args) {
// Bearing workBearing = new Bearing();
LatLonPointImpl pt1 = new LatLonPointImpl(40, -105);
LatLonPointImpl pt2 = new LatLonPointImpl(37.4, -118.4);
Bearing b = calculateBearing(pt1, pt2, null);
System.out.println("Bearing from " + pt1 + " to " + pt2 + " = \n\t" + b);
LatLonPointImpl pt3 = new LatLonPointImpl();
pt3 = findPoint(pt1, b.getAngle(), b.getDistance(), pt3);
System.out.println("using first point, angle and distance, found second point at " + pt3);
pt3 = findPoint(pt2, b.getBackAzimuth(), b.getDistance(), pt3);
System.out.println("using second point, backazimuth and distance, found first point at " + pt3);
/* uncomment for timing tests
for(int j=0;j<10;j++) {
long t1 = System.currentTimeMillis();
for(int i=0;i<30000;i++) {
workBearing = Bearing.calculateBearing(42.5,-93.0,
48.9,-117.09,workBearing);
}
long t2 = System.currentTimeMillis();
System.err.println ("time:" + (t2-t1));
}
*/
}
/**
* Calculate a position given an azimuth and distance from another point.
*
* @param e Earth object (defines radius and flattening)
* @param pt1 Point 1
* @param az azimuth (degrees)
* @param dist distance from the point (km)
* @param result Object to use if non-null
* @return The LatLonPoint
* @see #findPoint(double,double,double,double,LatLonPointImpl)
*/
public static LatLonPointImpl findPoint(
Earth e, LatLonPoint pt1, double az, double dist, LatLonPointImpl result) {
return findPoint(e, pt1.getLatitude(), pt1.getLongitude(), az, dist, result);
}
/**
* Calculate a position given an azimuth and distance from another point. Uses default Earth.
*
* @param pt1 Point 1
* @param az azimuth (degrees)
* @param dist distance from the point (km)
* @param result Object to use if non-null
* @return The LatLonPoint
* @see #findPoint(double,double,double,double,LatLonPointImpl)
*/
public static LatLonPointImpl findPoint(
LatLonPoint pt1, double az, double dist, LatLonPointImpl result) {
return findPoint(defaultEarth, pt1.getLatitude(), pt1.getLongitude(), az, dist, result);
}
/**
* Calculate a position given an azimuth and distance from another point. See details, below. Uses
* default Earth.
*
* @param lat1 latitude of starting point
* @param lon1 longitude of starting point
* @param az forward azimuth (degrees)
* @param dist distance from the point (km)
* @param result Object to use if non-null
* @return the position as a LatLonPointImpl
*/
public static LatLonPointImpl findPoint(
double lat1, double lon1, double az, double dist, LatLonPointImpl result) {
return findPoint(defaultEarth, lat1, lon1, az, dist, result);
}
/**
* Calculate a position given an azimuth and distance from another point.
*
* <p>
*
* <p>Algorithm from National Geodetic Survey, FORTRAN program "forward," subroutine "DIRCT1," by
* stephen j. frakes. http://www.ngs.noaa.gov/TOOLS/Inv_Fwd/Inv_Fwd.html
*
* <p>Original documentation:
*
* <pre>
* SOLUTION OF THE GEODETIC DIRECT PROBLEM AFTER T.VINCENTY
* MODIFIED RAINSFORD'S METHOD WITH HELMERT'S ELLIPTICAL TERMS
* EFFECTIVE IN ANY AZIMUTH AND AT ANY DISTANCE SHORT OF ANTIPODAL
* </pre>
*
* @param e Earth object (defines radius and flattening)
* @param lat1 latitude of starting point
* @param lon1 longitude of starting point
* @param az forward azimuth (degrees)
* @param dist distance from the point (km)
* @param result Object to use if non-null
* @return the position as a LatLonPointImpl
*/
public static LatLonPointImpl findPoint(
Earth e, double lat1, double lon1, double az, double dist, LatLonPointImpl result) {
if (result == null) {
result = new LatLonPointImpl();
}
if ((dist == 0)) {
result.setLatitude(lat1);
result.setLongitude(lon1);
return result;
}
A = e.getMajor();
F = e.getFlattening();
R = 1.0 - F;
// Algorithm from National Geodetic Survey, FORTRAN program "forward,"
// subroutine "DIRCT1," by stephen j. frakes.
// http://www.ngs.noaa.gov/TOOLS/Inv_Fwd/Inv_Fwd.html
// Conversion to JAVA from FORTRAN was made with as few changes as
// possible to avoid errors made while recasting form, and
// to facilitate any future comparisons between the original
// code and the altered version in Java.
// Original documentation:
// SUBROUTINE DIRCT1(GLAT1,GLON1,GLAT2,GLON2,FAZ,BAZ,S)
//
// SOLUTION OF THE GEODETIC DIRECT PROBLEM AFTER T.VINCENTY
// MODIFIED RAINSFORD'S METHOD WITH HELMERT'S ELLIPTICAL TERMS
// EFFECTIVE IN ANY AZIMUTH AND AT ANY DISTANCE SHORT OF ANTIPODAL
//
// A IS THE SEMI-MAJOR AXIS OF THE REFERENCE ELLIPSOID
// F IS THE FLATTENING OF THE REFERENCE ELLIPSOID
// LATITUDES AND LONGITUDES IN RADIANS POSITIVE NORTH AND EAST
// AZIMUTHS IN RADIANS CLOCKWISE FROM NORTH
// GEODESIC DISTANCE S ASSUMED IN UNITS OF SEMI-MAJOR AXIS A
//
// PROGRAMMED FOR CDC-6600 BY LCDR L.PFEIFER NGS ROCKVILLE MD 20FEB75
// MODIFIED FOR SYSTEM 360 BY JOHN G GERGEN NGS ROCKVILLE MD 750608
//
if (az < 0.0) {
az += 360.0; // reset azs from -180 to 180 to 0 to 360
}
double FAZ = az * rad;
double GLAT1 = lat1 * rad;
double GLON1 = lon1 * rad;
double S = dist * 1000.; // convert to meters
double TU = R * Math.sin(GLAT1) / Math.cos(GLAT1);
double SF = Math.sin(FAZ);
double CF = Math.cos(FAZ);
double BAZ = 0.;
if (CF != 0) {
BAZ = Math.atan2(TU, CF) * 2;
}
double CU = 1. / Math.sqrt(TU * TU + 1.);
double SU = TU * CU;
double SA = CU * SF;
double C2A = -SA * SA + 1.;
double X = Math.sqrt((1. / R / R - 1.) * C2A + 1.) + 1.;
X = (X - 2.) / X;
double C = 1. - X;
C = (X * X / 4. + 1) / C;
double D = (0.375 * X * X - 1.) * X;
TU = S / R / A / C;
double Y = TU;
double SY, CY, CZ, E, GLAT2, GLON2;
do {
SY = Math.sin(Y);
CY = Math.cos(Y);
CZ = Math.cos(BAZ + Y);
E = CZ * CZ * 2. - 1.;
C = Y;
X = E * CY;
Y = E + E - 1.;
Y = (((SY * SY * 4. - 3.) * Y * CZ * D / 6. + X) * D / 4. - CZ) * SY * D + TU;
} while (Math.abs(Y - C) > EPS);
BAZ = CU * CY * CF - SU * SY;
C = R * Math.sqrt(SA * SA + BAZ * BAZ);
D = SU * CY + CU * SY * CF;
GLAT2 = Math.atan2(D, C);
C = CU * CY - SU * SY * CF;
X = Math.atan2(SY * SF, C);
C = ((-3. * C2A + 4.) * F + 4.) * C2A * F / 16.;
D = ((E * CY * C + CZ) * SY * C + Y) * SA;
GLON2 = GLON1 + X - (1. - C) * D * F;
BAZ = (Math.atan2(SA, BAZ) + Math.PI) * deg;
result.setLatitude(GLAT2 * deg);
result.setLongitude(GLON2 * deg);
return result;
}
}