public double scanAngle(TBPLineSegment second, TBPoint interPoint)
{
Point3d p1, p2, p3;
TBPLineSegment s1 = null;
TBPLineSegment s2 = null;
if ((interPoint == this.StartPoint) && (this.StartPoint == second.StartPoint))
{
s1 = this.newReverse();
s2 = second;
}
else if ((interPoint == this.StartPoint) && (this.StartPoint == second.EndPoint))
{
s1 = this.newReverse();
s2 = second.newReverse();
}
else if ((interPoint == this.EndPoint) && (this.EndPoint == second.StartPoint))
{
s1 = this;
s2 = second;
}
else if ((interPoint == this.EndPoint) && (this.EndPoint == second.EndPoint))
{
s1 = this;
s2 = second.newReverse();
}
p1 = s1.acPline.GetPointAtParameter(this.acPline.EndParam - 1);
p2 = s1.EndPoint.acPoint;
p3 = s2.acPline.GetPointAtParameter(1);
return(AngleUtil.scanAngle(p1, p2, p3));
}
public void SingleAngle()
{
Vector v1 = new Vector(0, 1);
double angle = VectorUtil.Angle(v1);
Assert.AreEqual(90, AngleUtil.ToDegrees(angle));
}
public void Angle()
{
Vector v1 = new Vector(0, 1);
Vector v2 = new Vector(1, -1);
double angle = VectorUtil.Angle(v1, v2);
Assert.AreEqual(135, AngleUtil.ToDegrees(angle));
}
public override void Invoke(CollisionInfo collision)
{
var position = spawnAtAvatarBone && AvatarController.TryGetBone(contextBone, out var bone)
? bone.position
: collision.point;
Invoke(position, AngleUtil.DirectionalRotation(collision.normal));
}
public override MovementCalculator CreateMovementCalculator(AvatarController avatar)
{
float angularVelocity = 0;
return(deltaTime => {
var velocityOrientation = new AvatarOrientation(avatar.physics.velocityAngle);
var currentOrientation = new AvatarOrientation(avatar.physics.rotationAngle);
var velocity = avatar.physics.velocity;
var direction = avatar.intentions.intendedFlight == Vector2.zero
? avatar.physics.rotatedForward
: avatar.intentions.intendedFlight.normalized;
float angle = Vector2.SignedAngle(avatar.physics.forward, direction);
if (!avatar.canFlyLooping)
{
//angle = Mathf.Clamp(angle, -90, 90);
}
var oldRotation = currentOrientation.rotation;
currentOrientation.angle = Mathf.SmoothDampAngle(currentOrientation.angle, angle, ref angularVelocity, rotationDuration, maxRotationSpeed, deltaTime);
// Gliding
float alignment = AngleUtil.Alignment(velocityOrientation.rotation, currentOrientation.rotation);
bool isAligned = alignment >= alignmentDeadZone;
if (isAligned && Mathf.Abs(angularVelocity) > angularDeadZone)
{
velocity = velocity.magnitude * (currentOrientation.rotation * avatar.physics.forward).SwizzleXY();
avatar.broom.isBoosting = true;
}
else
{
avatar.broom.isBoosting = false;
}
// Drag
avatar.physics.drag = GetDrag(alignment);
velocity += avatar.physics.dragStep;
// Gravity
velocity += avatar.physics.gravityStep;
// Thrust
if (isAligned)
{
float thrustAcceleration = GetThrust(alignment);
var thrustDirection = (currentOrientation.rotation * avatar.physics.forward).SwizzleXY();
velocity += thrustAcceleration * deltaTime * thrustDirection;
}
// Visuals
avatar.broom.isAligned = isAligned;
return (velocity, currentOrientation.angle, avatar.physics.facing);
});
}
//==========================================================================================
//
// Called once per frame. If we've connected to a tracker, get its data and move attached object with device
//
//==========================================================================================
void Update()
{
if (handle > 0)
{
if (sync)
{ // Try to predict
ISenseLib.ISD_STATION_DATA_TYPE bufferData = new ISenseLib.ISD_STATION_DATA_TYPE();
ISenseLib.ISD_RingBufferQuery(handle, station, ref bufferData, new IntPtr(), new IntPtr());
float dT = (float)Time.deltaTime;
float curTime = bufferData.TimeStamp; //TODO: is this right?
float timeToPredict = curTime + (framePredict * dT);
GetRingBufferDataAtTime(timeToPredict);
}
else
{
ISenseLib.ISD_GetTrackingData(handle, ref data); // Get the tracking data
}
ISenseLib.ISD_STATION_DATA_TYPE dataSet = data.Station[station - 1]; // Specify the dataset
// Only IS-1200 and IS-1500 have trackState
if (trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1200 || trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1500)
{
trackState = dataSet.TrackingState;
}
Vector3 pos;
// If device can control position, control position!
if (trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS600 ||
trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS900 ||
trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1200 ||
trackerModel == ISenseLib.ISD_SYSTEM_MODEL.ISD_IS1500)
{
pos = new Vector3(dataSet.Position[1], -dataSet.Position[2], dataSet.Position[0]); // Their y is up/down while that's our z
transform.localPosition = pos;
}
// Time to deal with rotation!
float[] cbn = dataSet.Cbn;
float[] eulers;
cbn = AngleUtil.ConvertUnityCbn(cbn); // Converts our right-handed rotation frame to unity's left-handed one.
eulers = AngleUtil.Cbn2Euler(cbn);
transform.localRotation = Quaternion.Euler(eulers[1], eulers[2], eulers[0]); // Pitch, Yaw, Roll
}
}
void Dispatch(ContactPoint2D contact)
{
if (contact.normalImpulse < impulseThreshold)
{
return;
}
var tempObject = new GameObject();
tempObject.transform.SetPositionAndRotation(contact.point, AngleUtil.DirectionalRotation(contact.normal));
onColliderEnter.Invoke(tempObject);
Destroy(tempObject, 0);
}
IEnumerable <GameObject> CreateContactObjects(Collision2D collision)
{
var contacts = collision.contacts;
for (int i = 0; i < contacts.Length; i++)
{
if (contacts[i].normalImpulse >= onCollisionImpulseThreshold)
{
var tempObject = new GameObject();
tempObject.transform.SetPositionAndRotation(contacts[i].point, AngleUtil.DirectionalRotation(contacts[i].normal));
Destroy(tempObject, 0);
yield return(tempObject);
}
}
}
public double scanAngle()
{
Document acDoc = Application.DocumentManager.MdiActiveDocument;
Database acCurDb = acDoc.Database;
double r = 0;
//using (Transaction trans = acCurDb.TransactionManager.StartTransaction())
//{
//Polyline3d pl = trans.GetObject(acPline.ObjectId, OpenMode.ForRead) as Polyline3d;
for (int i = 0; i < acPline.EndParam - 1; i++)
{
r += AngleUtil.scanAngle(acPline.GetPointAtParameter(i), acPline.GetPointAtParameter(i + 1), acPline.GetPointAtParameter(i + 2));
}
//}
return(r);
}
// ---- Rotation ----
void updateRotation()
{
if (_inputState.rotationEnabled)
{
_normalizedRotation = _rotationLimits.InverseLerp(_currRotation);
_normalizedRotation = _rotationSpeedEasing.Evaluate2(_normalizedRotation);
_currRotation += _inputState.rotation * _rotationSpeed.Lerp(_normalizedRotation) * Time.deltaTime;
}
else
{
// return rotation
if (_currRotation != _returnRotation)
{
if ((_returnRotationSpeed.min.x > 0 && _returnRotationSpeed.max.x > 0) || (_returnRotationSpeed.min.y > 0 && _returnRotationSpeed.max.y > 0))
{
//how far we are away from the return rotation
_normalizedRotation -= _returnRotation;
_normalizedRotation.x = Mathf.Abs(AngleUtil.ClampAngle(_normalizedRotation.x, -_returnRotSpeedDist.x, _returnRotSpeedDist.x) / _returnRotSpeedDist.x);
_normalizedRotation.y = Mathf.Abs(AngleUtil.ClampAngle(_normalizedRotation.y, -_returnRotSpeedDist.y, _returnRotSpeedDist.y) / _returnRotSpeedDist.y);
_normalizedRotation = _returnRotationSpeedEasing.Evaluate2(_normalizedRotation);
// now modify the current rotation to move towards the return rotation
if (_returnRotationSpeed.min.x > 0 && _returnRotationSpeed.max.x > 0)
{
_currRotation.x = Mathf.Lerp(_currRotation.x, _returnRotation.x, Time.deltaTime * _returnRotationSpeed.getLerpedX(_normalizedRotation.x));
}
if (_returnRotationSpeed.min.y > 0 && _returnRotationSpeed.max.y > 0)
{
_currRotation.y = Mathf.Lerp(_currRotation.y, _returnRotation.y, Time.deltaTime * _returnRotationSpeed.getLerpedY(_normalizedRotation.y));
}
}
}
}
// clamp the rotation angles
_currRotation.x = AngleUtil.ClampAngle(_currRotation.x, _rotationLimits.min.x, _rotationLimits.max.x);
_currRotation.y = AngleUtil.ClampAngle(_currRotation.y, _rotationLimits.min.y, _rotationLimits.max.y);
// set camera rotation, smoothed
_desiredRotation = Quaternion.Euler((_currRotation.y * _upDir) + (_currRotation.x * _rightDir));
transform.rotation = Quaternion.Lerp(transform.rotation, _desiredRotation, Time.deltaTime * _rotationDampening);
}
public void TestDirectionalAngle(float x, float y, float angle)
{
Assert.AreEqual(angle, AngleUtil.DirectionalAngle(new Vector2(x, y)), Mathf.Epsilon);
}
public void TestNormalizeAngle(float angle, float normalizedAngle)
{
Assert.AreEqual(normalizedAngle, AngleUtil.NormalizeAngle(angle));
}
public void TestDirectionalRotation(float x, float y, float angleX, float angleY, float angleZ)
{
Assert.AreEqual(Quaternion.Euler(angleX, angleY, angleZ), AngleUtil.DirectionalRotation(new Vector2(x, y)));
}
public override MovementCalculator CreateMovementCalculator(AvatarController avatar)
{
float angularVelocity = 0;
var acceleration = Vector2.zero;
float boostGatheringDuration = 0;
float boostExecutionDuration = 0;
float maximumBoostExecutionDuration = 0;
return(deltaTime => {
float currentAngle = avatar.physics.rotationAngle;
var targetDirection = avatar.intentions.intendedFlight == Vector2.zero
? avatar.physics.rotatedForward
: avatar.intentions.intendedFlight;
float targetAngle = Vector2.SignedAngle(avatar.physics.forward, targetDirection);
if (directionsNormalized)
{
targetAngle = AngleUtil.RoundAngle(targetAngle, directionRange);
}
var currentVelocity = avatar.physics.velocity;
(Vector2, float, int) boost()
{
avatar.physics.drag = boostDrag;
boostExecutionDuration++;
if (boostExecutionDuration >= maximumBoostExecutionDuration)
{
boostExecutionDuration = 0;
avatar.broom.isBoosting = false;
angularVelocity = 0;
return glide();
}
float angle = Mathf.SmoothDampAngle(currentAngle, targetAngle, ref angularVelocity, boostRotationDuration, maximumRotationSpeed, deltaTime);
var rotation = Quaternion.Euler(0, 0, angle);
var targetVelocity = (Vector2)(rotation * Vector2.right * currentVelocity.magnitude);
var velocity = targetVelocity;
return (velocity, angle, avatar.physics.facing);
}
(Vector2, float, int) glide()
{
float angle = Mathf.SmoothDampAngle(currentAngle, targetAngle, ref angularVelocity, rotationDuration, maximumRotationSpeed, deltaTime);
var rotation = Quaternion.Euler(0, 0, angle);
var velocityRotation = AngleUtil.DirectionalRotation(currentVelocity);
float alignment = AngleUtil.Alignment(rotation, velocityRotation);
float alignDuration = GetAlignDuration(alignment);
var targetVelocity = rotation * Vector2.right * currentVelocity.magnitude;
avatar.physics.drag = GetDrag(alignment);
avatar.broom.isAligned = alignment > requiredAlignment;
avatar.broom.isDiving = currentVelocity.y < requiredYSpeed;
bool intendsBoost = targetAngle <= 180;
if (intendsBoost)
{
if (avatar.broom.canBoost)
{
avatar.broom.canBoost = false;
avatar.broom.isBoosting = true;
maximumBoostExecutionDuration = Mathf.Abs(currentVelocity.y) * boostExecutionDurationMultiplier;
onBoost.Invoke(avatar.gameObject);
currentVelocity += currentVelocity.normalized * boostExecutionSpeed;
angularVelocity = 0;
return boost();
}
}
else
{
if (avatar.broom.isAligned && avatar.broom.isDiving)
{
if (!avatar.broom.canBoost && boostGatheringDuration < minimumBoostGatheringDuration)
{
boostGatheringDuration += deltaTime;
if (boostGatheringDuration >= minimumBoostGatheringDuration)
{
boostGatheringDuration = 0;
avatar.broom.canBoost = true;
}
}
}
}
var velocity = Vector2.SmoothDamp(currentVelocity, targetVelocity, ref acceleration, alignDuration, maximumGlideAcceleration, deltaTime);
velocity += GetAcceleration(alignment) * Time.deltaTime * (Vector2)(rotation * Vector3.right);
velocity += avatar.physics.gravityStep;
return (velocity, angle, avatar.physics.facing);
}
return avatar.broom.isBoosting
? boost()
: glide();
});
}
public void TestHorizontalAngle(int sign, float angle)
{
Assert.AreEqual(angle, AngleUtil.HorizontalAngle(sign));
}
public void TestHorizontalSign(float x, float y, int sign)
{
Assert.AreEqual(sign, AngleUtil.HorizontalSign(new Vector2(x, y)));
}
public void Angle()
{
Vector v1 = new Vector(1, 1);
Assert.AreEqual(45, AngleUtil.ToDegrees(v1.Angle));
}
public void TestHorizontalAngleThrowsException()
{
Assert.Throws <UnityEngine.Assertions.AssertionException>(() => AngleUtil.HorizontalAngle(0));
}
public void TestAlignment(float angleA, float angleB, float alignment)
{
Assert.AreEqual(alignment, AngleUtil.Alignment(Quaternion.Euler(0, 0, angleA), Quaternion.Euler(0, 0, angleB)), 0.001f);
}
public void Init()
{
//set the current distance and rotation so we keep our initial position
setDistanceToTarget();
//figure out the direction to aim in.
switch (_direction)
{
default:
_forwardDir = Vector3.forward;
_upDir = Vector3.right;
_rightDir = Vector3.up;
_currRotation.x = AngleUtil.Normalize(transform.eulerAngles.y);
_currRotation.y = AngleUtil.Normalize(transform.eulerAngles.x);
break;
case DIRECTION.RIGHT:
_forwardDir = Vector3.right;
_upDir = Vector3.forward;
_rightDir = Vector3.up;
_currRotation.x = AngleUtil.Normalize(transform.eulerAngles.y);
_currRotation.y = AngleUtil.Normalize(transform.eulerAngles.z);
break;
case DIRECTION.UP:
_forwardDir = Vector3.up;
_upDir = Vector3.right;
_rightDir = Vector3.forward;
_currRotation.x = AngleUtil.Normalize(transform.eulerAngles.z);
_currRotation.y = AngleUtil.Normalize(transform.eulerAngles.x);
break;
case DIRECTION.BACKWARD:
_forwardDir = -Vector3.forward;
_upDir = Vector3.right;
_rightDir = Vector3.up;
_currRotation.x = AngleUtil.Normalize(transform.eulerAngles.y);
_currRotation.y = AngleUtil.Normalize(transform.eulerAngles.x);
break;
case DIRECTION.LEFT:
_forwardDir = -Vector3.right;
_upDir = Vector3.forward;
_rightDir = Vector3.up;
_currRotation.x = AngleUtil.Normalize(transform.eulerAngles.y);
_currRotation.y = AngleUtil.Normalize(transform.eulerAngles.z);
break;
case DIRECTION.DOWN:
_forwardDir = -Vector3.up;
_upDir = Vector3.right;
_rightDir = Vector3.forward;
_currRotation.x = AngleUtil.Normalize(transform.eulerAngles.z);
_currRotation.y = AngleUtil.Normalize(transform.eulerAngles.x);
break;
}
}
public void TestRoundAngle(float angle, int directions, float roundedAngle)
{
Assert.AreEqual(roundedAngle, AngleUtil.RoundAngle(angle, directions));
}
