User:Pedersen/Code Samples/getLateralVelocity

From Robowiki
Jump to navigation Jump to search

The following code snippet is what I presently (v0.14) use for segmentation on lateral (tangental) velocity. The result is a number between -1.0 and 1.0, representing maximum left or right orbit, respectively. The maximum would be attained at full speed completely tantental to my position. (Actually, this isn't exactly true. Turning slightly towards me is a hair faster.) The code includes a lot of my physics framework, but it should be readable.

	private static double getTangentalVelocity( StaticPosition firingPosition, Snapshot target )
	{
		double bearingToTarget = firingPosition.getBearing( target ); // [0,2pi)
		StaticPosition projectedTarget = new Projection( target ).project().getStaticPosition();
                // the control position is 8.0 units to the right (tangentally) of the present target position
		StaticPosition controlPosition = new StaticPositionImpl( target, bearingToTarget + Constraints.rightAngle, Constraints.maxAbsVehicleVelocity );
		double bearingToProjectedTarget = Constraints.getNegativePiToPi( firingPosition.getBearing( projectedTarget ) - bearingToTarget );
		double bearingToControlPosition = Constraints.getNegativePiToPi( firingPosition.getBearing( controlPosition ) - bearingToTarget );
		double tangentalVelocity = bearingToProjectedTarget / bearingToControlPosition;
		return tangentalVelocity;
	}