diff --git a/src/helpers/curveHelper.js b/src/helpers/curveHelper.js index 55d43e7..74a165c 100644 --- a/src/helpers/curveHelper.js +++ b/src/helpers/curveHelper.js @@ -1,4 +1,6 @@ import Vector3 from "../model/vector3"; +import Quaternion from "../model/quaternion"; +import AngleHelper from "./angleHelper"; function calculateCurveEnd(startPos, startAngle, radius, curveLength) { if (radius === 0) { @@ -34,9 +36,24 @@ function calculateCurveEndStandard(radius, curveLength) { return { endPos, endAngle, circleCenter }; } +function transformStart(globalStart, globalRotationDeg, localStart, localRotationQuat) { + const globalRotationRad = AngleHelper.rotationDegToRad(globalRotationDeg); + const startPos = globalStart.add(localStart.rotate(globalRotationRad)); + const rotationQuat = Quaternion.fromEulerAnglesRad(globalRotationRad).multiply(localRotationQuat); + const rotationRad = rotationQuat.toEulerAnglesRad(); + const rotationDeg = AngleHelper.rotationRadToDeg(rotationRad); + + return { + startPos, + rotationQuat, + rotationDeg, + }; +} + const CurveHelper = { calculateCurveEnd, calculateCurveEndStandard, + transformStart, }; export default CurveHelper; diff --git a/src/model/quaternion.js b/src/model/quaternion.js index 48d6f09..1f59e6c 100644 --- a/src/model/quaternion.js +++ b/src/model/quaternion.js @@ -1,3 +1,5 @@ +import Vector3 from "./vector3"; + export default class Quaternion { x; y; @@ -24,4 +26,42 @@ export default class Quaternion { static fromVec(vec) { return new Quaternion(vec.x, vec.y, vec.z, 0); } + + /** + * Convert a Unity rotation vector (extrinsic z-x-y) in radians to a quaternion. + */ + static fromEulerAnglesRad(vec) { + const cosX = Math.cos(vec.x/2); + const sinX = Math.sin(vec.x/2); + const cosY = Math.cos(vec.y/2); + const sinY = Math.sin(vec.y/2); + const cosZ = Math.cos(vec.z/2); + const sinZ = Math.sin(vec.z/2); + + return new Quaternion( + cosY * sinX * cosZ + sinY * cosX * sinZ, + sinY * cosX * cosZ - cosY * sinX * sinZ, + cosY * cosX * sinZ - sinY * sinX * cosZ, + cosY * cosX * cosZ + sinY * sinX * sinZ, + ); + } + + toEulerAnglesRad() { + const sinX = Math.min(Math.max(-1, 2 * (this.w * this.x - this.y * this.z)), 1); + const xRad = Math.asin(sinX); + const yRad = Math.atan2( + 2 * (this.x * this.z + this.w * this.y), + 1 - 2 * (this.x * this.x + this.y * this.y) + ); + + if (Math.abs(sinX) > 1 - 1e-6) { + return new Vector3(xRad, yRad, 0); + } + + const zRad = Math.atan2( + 2 * (this.x * this.y + this.w * this.z), + 1 - 2 * (this.x * this.x + this.z * this.z) + ); + return new Vector3(xRad, yRad, zRad); + } } diff --git a/src/model/switch.js b/src/model/switch.js index 7610956..4ca5733 100644 --- a/src/model/switch.js +++ b/src/model/switch.js @@ -1,4 +1,3 @@ -import PointTrack from "./tracks/point-track.js"; import SceneryObject from "./scenery-object.js"; import SceneryParserLog from "./scenery-parser-log.js"; import Vector3 from "./vector3.js"; @@ -6,6 +5,7 @@ import {DefinedSwitches, SwitchTrackConnectionType} from "./defs/defined-switche import TrackConnection, {TrackConnectionEnd} from "./track-connection"; import CurveHelper from "../helpers/curveHelper"; import AngleHelper from "../helpers/angleHelper"; +import StandardTrack from "./tracks/standard-track"; export default class Switch extends SceneryObject { model; @@ -142,7 +142,6 @@ export default class Switch extends SceneryObject { } _createSwitchTrackFromDef(scenery, switchDef, trackDef, ids) { - const rotRad = AngleHelper.rotationDegToRad(this.rot); if (trackDef.dataIndex >= ids.length) { SceneryParserLog.warn( 'switchMissingTrackId', @@ -152,31 +151,20 @@ export default class Switch extends SceneryObject { } const trackId = ids[trackDef.dataIndex][0]; const connections = this._createTrackConnections(switchDef, trackDef, ids); + const { startPos, rotationDeg } = CurveHelper.transformStart(this.pos, this.rot, trackDef.pos, trackDef.rot); - const localEndPos = CurveHelper - .calculateCurveEndStandard(trackDef.radius, trackDef.length) - .endPos - .rotateByQuaternion(trackDef.rot) - .add(trackDef.pos); - - const trackObj = new PointTrack( + const track = StandardTrack.switch( trackId, - this.pos.add(trackDef.pos.rotate(rotRad)), - this.pos.add(localEndPos.rotate(rotRad)), + startPos, + rotationDeg, + trackDef.length, trackDef.radius, connections, - this.id_switch, - 0, // start_slope - 0, // end_slope - this.id_isolation, - this.track_prefab_name, - this.maxspeed, - this.derailspeed + trackDef.slope1, + trackDef.slope2, + this, ); - - trackObj.switch = this; - scenery.addObject(trackObj); - - return trackObj; + scenery.addObject(track); + return track; } } diff --git a/src/model/tracks/point-track.js b/src/model/tracks/point-track.js deleted file mode 100644 index 895df26..0000000 --- a/src/model/tracks/point-track.js +++ /dev/null @@ -1,53 +0,0 @@ -import AngleHelper from '../../helpers/angleHelper'; -import Track, {TrackSource} from './track'; -import Vector3 from '../vector3'; - -export default class PointTrack extends Track -{ - type = "PointTrack"; - points = { - start: Vector3.zero(), - end: Vector3.zero() - }; - - constructor(id, start, end, r, connections, id_station, start_slope, end_slope, id_isolation, prefab_name, maxspeed, derailspeed) { - const [rot, len] = PointTrack._getRotLen(start, end, r); - super(id, start, rot, len, r, connections, id_station, start_slope, end_slope, id_isolation, prefab_name, maxspeed, derailspeed, TrackSource.SWITCH); - - Object.assign(this.points, { - start, end - }); - } - - getStartAngleXZ() { - return AngleHelper.degToRad(this.rot.y); - } - - getEndAngleXZ() { - const startAngle = AngleHelper.degToRad(this.rot.y); - - if(this.r === 0) { - return startAngle; - } - - return startAngle - this.len / this.r; - } - - static _getRotLen(start, end, r) { - // TODO: include track slope in calculations - const pointsAngle = start.atanY(end); - - if(r === 0) { - const rot = new Vector3(0, AngleHelper.radToDeg(pointsAngle), 0); - const len = start.distance(end); - - return [rot, len]; - } - - const len = Math.abs(2 * r * Math.asin(start.distance(end) / (2 * r))); - const ry = pointsAngle + len / (r * 2); - - const rot = new Vector3(0, AngleHelper.radToDeg(ry), 0); - return [rot, len]; - } -} diff --git a/src/model/tracks/standard-track.js b/src/model/tracks/standard-track.js index 6f0f54a..e0d6bad 100644 --- a/src/model/tracks/standard-track.js +++ b/src/model/tracks/standard-track.js @@ -107,4 +107,21 @@ export default class StandardTrack extends Track { track.route = route; return track; } + + static switch(id, start, rot, len, r, connections, start_slope, end_slope, object) { + const track = new StandardTrack( + id, start, rot, len, r, + connections, + object.id_switch, + start_slope, + end_slope, + object.id_isolation, + object.prefab_name, + object.maxspeed, + object.derailspeed, + TrackSource.SWITCH, + ); + track.switch = object; + return track; + } }