| public static Atan2 ( float y, float x ) : float | ||
| y | float | |
| x | float | |
| return | float | |
static void DrawVO(Vector2 circleCenter, float radius, Vector2 origin)
{
float alpha = Mathf.Atan2((origin - circleCenter).y, (origin - circleCenter).x);
float gamma = radius / (origin - circleCenter).magnitude;
float delta = gamma <= 1.0f ? Mathf.Abs(Mathf.Acos(gamma)) : 0;
Draw.Debug.CircleXZ(FromXZ(circleCenter), radius, Color.black, alpha - delta, alpha + delta);
Vector2 p1 = new Vector2(Mathf.Cos(alpha - delta), Mathf.Sin(alpha - delta)) * radius;
Vector2 p2 = new Vector2(Mathf.Cos(alpha + delta), Mathf.Sin(alpha + delta)) * radius;
Vector2 p1t = -new Vector2(-p1.y, p1.x);
Vector2 p2t = new Vector2(-p2.y, p2.x);
p1 += circleCenter;
p2 += circleCenter;
Debug.DrawRay(FromXZ(p1), FromXZ(p1t).normalized *100, Color.black);
Debug.DrawRay(FromXZ(p2), FromXZ(p2t).normalized *100, Color.black);
}
/// <summary>
/// Convert matrix into euler degree rotation
/// </summary>
/// <remarks>
/// The method is not valid in 90 deg singularity (WIP).
/// The method won't check for orthogonality.
/// </remarks>
public Euler4 ToEuler()
{
// Singularity check.
//var d = Diagonal; var t = d.x * d.y * d.z * d.w; var t2 = Vector4.Dot(d, Vector4.one); t2 = t2 * t2;
//if (t > 0.99F) return ToEulerInSingularity(); // Because Utility.Atan2AvoidPi doesn't care about 180 deg singularities...
// if (t2 - 1 < 0.001F) return ToEulerInHalfSingularity(); // 90 deg is bad. Some cos() parameters got zero, and could results in chaos because of losing sign information
// Back to main topic..
// Based on Euler() sequence, the raw formula is...
// matrix{{ach, dh, -bch, -j}, {k(bg-adf)-acjl, cfk-djl, k(bdf+ag)+bcjl, -hl}, {-ln(bg-adf)+m(adg+bf)-acjkn, -cgm-cfln-djkn, -ln(bdf+ag)+m(af-bdg)+bcjkn, -hkn}, {lm(bg-adf)+n(adg+bf)+acjkm, cflm+djkm-cgn, lm(bdf+ag)+n(af-bdg)-bcjkm, hkm}}
// matrix{{ach, -dh, bch, -j}, {k(adf+bg)-acjl, cfk+djl, k(bdf-ag)-bcjl, -hl},
// { -ln(adf+bg)+m(adg-bf)-acjkn, cgm+djkn-cfln, -ln(bdf-ag)+m(bdg+af)-bcjkn, -hkn},
// { lm(adf+bg)+n(adg-bf)+acjkm, cflm-djkm+cgn, lm(bdf-ag)+n(bdg+af)+bcjkm, hkm}}
Euler4 result = new Euler4();
result.y = AngleTidy(Math.Atan2(this[0, 2], this[0, 0])); // ach, bch
result.v = AngleTidy(Math.Atan2(-this[2, 3], this[3, 3])); // hkm, -hkn
// (So lucky) We got the first and last matrix sequence. Now make the matrix simpler
// Simplify into : matrix{{ch, dh, 0, -j}, {-cjl-dfk, cfk-djl, gk, -hl}, {dg, -cg, f, 0}, {cjk-dfl, cfl+djk, gl, hk}}
// matrix{{ch, -dh, 0, -j}, {dfk-cjl, cfk+djl, -gk, -hl}, {dg, cg, f, 0}, {cjk+dfl, cfl-djk, -gl, hk}}
var m2 = Euler(5, -result.v) * this * Euler(1, -result.y);
result.z = AngleTidy(Math.Atan2(-m2[0, 1], m2[0, 0])); // ch, -dh
result.u = AngleTidy(Math.Atan2(-m2[1, 3], m2[3, 3])); // hk, -hl
// Idk if it's safe to get x and t from that matrix, but working > efficient
// Simplify into : matrix{{h, 0, 0, -j}, {0, f, g, 0}, {0, -g, f, 0}, {j, 0, 0, h}}
var m3 = Euler(4, -result.u) * m2 * Euler(2, -result.z);
result.x = AngleTidy(Math.Atan2(m3[2, 1], m3[1, 1])); // f, g
result.t = AngleTidy(Math.Atan2(-m3[0, 3], m3[0, 0])); // h, -j
return(result);
}
public override void OnGUI()
{
base.OnGUI();
if (selectedObject == null)
{
return;
}
Texture currentTexture;
float markerX, markerY, markerRot;
Vector3 screenPos = Player.main.viewModelCamera.WorldToScreenPoint(selectedObject.transform.position);
//if object is on screen
if (screenPos.z > 0 &&
screenPos.x >= 0 && screenPos.x < Screen.width &&
screenPos.y >= 0 && screenPos.y < Screen.height)
{
currentTexture = circleTexture;
markerX = screenPos.x;
//subtract from height to go from bottom up to top down
markerY = Screen.height - screenPos.y;
markerRot = 0;
}
//if object is not on screen
else
{
currentTexture = arrowTexture;
//if the object is behind us, flip across the center
if (screenPos.z < 0)
{
screenPos.x = Screen.width - screenPos.x;
screenPos.y = Screen.height - screenPos.y;
}
//calculate new position of arrow (somewhere on the edge)
Vector3 screenCenter = new Vector3(Screen.width, Screen.height, 0) / 2f;
Vector3 originPos = screenPos - screenCenter;
float angle = Mathf.Atan2(originPos.y, originPos.x) - (90 * Mathf.Deg2Rad);
float cos = Mathf.Cos(angle);
float sin = Mathf.Sin(angle);
float m = cos / -sin;
Vector3 screenBounds = screenCenter * 0.9f;
if (cos > 0)
{
screenPos = new Vector3(screenBounds.y / m, screenBounds.y, 0);
}
else
{
screenPos = new Vector3(-screenBounds.y / m, -screenBounds.y, 0);
}
if (screenPos.x > screenBounds.x)
{
screenPos = new Vector3(screenBounds.x, screenBounds.x * m, 0);
}
else if (screenPos.x < -screenBounds.x)
{
screenPos = new Vector3(-screenBounds.x, -screenBounds.x * m, 0);
}
screenPos += screenCenter;
markerX = screenPos.x;
markerY = Screen.height - screenPos.y;
markerRot = -angle * Mathf.Rad2Deg;
}
float markerSizeX = currentTexture.width;
float markerSizeY = currentTexture.height;
GUI.matrix = Matrix4x4.Translate(new Vector3(markerX, markerY, 0)) *
Matrix4x4.Rotate(Quaternion.Euler(0, 0, markerRot)) *
Matrix4x4.Scale(new Vector3(0.5f, 0.5f, 0.5f)) *
Matrix4x4.Translate(new Vector3(-markerSizeX / 2, -markerSizeY / 2, 0));
GUI.DrawTexture(new Rect(0, 0, markerSizeX, markerSizeY), currentTexture);
GUI.matrix = Matrix4x4.identity;
}
/** Creates a VO for avoiding another agent.
* \param center The position of the other agent relative to this agent.
* \param offset Offset of the velocity obstacle. For example to account for the agents' relative velocities.
* \param radius Combined radius of the two agents (radius1 + radius2).
* \param inverseDt 1 divided by the local avoidance time horizon (e.g avoid agents that we will hit within the next 2 seconds).
* \param inverseDeltaTime 1 divided by the time step length.
*/
public VO(Vector2 center, Vector2 offset, float radius, float inverseDt, float inverseDeltaTime)
{
// Adjusted so that a parameter weightFactor of 1 will be the default ("natural") weight factor
this.weightFactor = 1;
weightBonus = 0;
//this.radius = radius;
Vector2 globalCenter;
circleCenter = center * inverseDt + offset;
this.weightFactor = 4 * Mathf.Exp(-Sqr(center.sqrMagnitude / (radius * radius))) + 1;
// Collision?
if (center.magnitude < radius)
{
colliding = true;
// 0.001 is there to make sure lin1.magnitude is not so small that the normalization
// below will return Vector2.zero as that will make the VO invalid and it will be ignored.
line1 = center.normalized * (center.magnitude - radius - 0.001f) * 0.3f * inverseDeltaTime;
dir1 = new Vector2(line1.y, -line1.x).normalized;
line1 += offset;
cutoffDir = Vector2.zero;
cutoffLine = Vector2.zero;
dir2 = Vector2.zero;
line2 = Vector2.zero;
this.radius = 0;
}
else
{
colliding = false;
center *= inverseDt;
radius *= inverseDt;
globalCenter = center + offset;
// 0.001 is there to make sure cutoffDistance is not so small that the normalization
// below will return Vector2.zero as that will make the VO invalid and it will be ignored.
var cutoffDistance = center.magnitude - radius + 0.001f;
cutoffLine = center.normalized * cutoffDistance;
cutoffDir = new Vector2(-cutoffLine.y, cutoffLine.x).normalized;
cutoffLine += offset;
float alpha = Mathf.Atan2(-center.y, -center.x);
float delta = Mathf.Abs(Mathf.Acos(radius / center.magnitude));
this.radius = radius;
// Bounding Lines
// Point on circle
line1 = new Vector2(Mathf.Cos(alpha + delta), Mathf.Sin(alpha + delta));
// Vector tangent to circle which is the correct line tangent
// Note that this vector is normalized
dir1 = new Vector2(line1.y, -line1.x);
// Point on circle
line2 = new Vector2(Mathf.Cos(alpha - delta), Mathf.Sin(alpha - delta));
// Vector tangent to circle which is the correct line tangent
// Note that this vector is normalized
dir2 = new Vector2(line2.y, -line2.x);
line1 = line1 * radius + globalCenter;
line2 = line2 * radius + globalCenter;
}
segmentStart = Vector2.zero;
segmentEnd = Vector2.zero;
segment = false;
}
