/** Clamps the velocity to the max speed and optionally the forwards direction.
* \param velocity Desired velocity of the character. In world units per second.
* \param maxSpeed Max speed of the character. In world units per second.
* \param slowdownFactor Value between 0 and 1 which determines how much slower the character should move than normal.
* Normally 1 but should go to 0 when the character approaches the end of the path.
* \param slowWhenNotFacingTarget Prevent the velocity from being too far away from the forward direction of the character
* and slow the character down if the desired velocity is not in the same direction as the forward vector.
* \param forward Forward direction of the character. Used together with the \a slowWhenNotFacingTarget parameter.
*
* Note that all vectors are 2D vectors, not 3D vectors.
*
* \returns The clamped velocity in world units per second.
*/
public static Vector2 ClampVelocity(Vector2 velocity, float maxSpeed, float slowdownFactor, bool slowWhenNotFacingTarget, Vector2 forward)
{
// Max speed to use for this frame
var currentMaxSpeed = maxSpeed * slowdownFactor;
// Check if the agent should slow down in case it is not facing the direction it wants to move in
if (slowWhenNotFacingTarget && (forward.x != 0 || forward.y != 0))
{
float currentSpeed;
var normalizedVelocity = VectorMath.Normalize(velocity.ToPFV2(), out currentSpeed);
float dot = Vector2.Dot(normalizedVelocity.ToUnityV2(), forward);
// Lower the speed when the character's forward direction is not pointing towards the desired velocity
// 1 when velocity is in the same direction as forward
// 0.2 when they point in the opposite directions
float directionSpeedFactor = Mathf.Clamp(dot + 0.707f, 0.2f, 1.0f);
currentMaxSpeed *= directionSpeedFactor;
currentSpeed = Mathf.Min(currentSpeed, currentMaxSpeed);
// Angle between the forwards direction of the character and our desired velocity
float angle = Mathf.Acos(Mathf.Clamp(dot, -1, 1));
// Clamp the angle to 20 degrees
// We cannot keep the velocity exactly in the forwards direction of the character
// because we use the rotation to determine in which direction to rotate and if
// the velocity would always be in the forwards direction of the character then
// the character would never rotate.
// Allow larger angles when near the end of the path to prevent oscillations.
angle = Mathf.Min(angle, (20f + 180f * (1 - slowdownFactor * slowdownFactor)) * Mathf.Deg2Rad);
float sin = Mathf.Sin(angle);
float cos = Mathf.Cos(angle);
// Determine if we should rotate clockwise or counter-clockwise to move towards the current velocity
sin *= Mathf.Sign(normalizedVelocity.x * forward.y - normalizedVelocity.y * forward.x);
// Rotate the #forward vector by #angle radians
// The rotation is done using an inlined rotation matrix.
// See https://en.wikipedia.org/wiki/Rotation_matrix
return(new Vector2(forward.x * cos + forward.y * sin, forward.y * cos - forward.x * sin) * currentSpeed);
}
else
{
return(Vector2.ClampMagnitude(velocity, currentMaxSpeed));
}
}
public void Update(IChiselHandles handles, ChiselCylinderDefinition definition)
{
var tempBottomDiameterX = definition.BottomDiameterX;
var tempBottomDiameterZ = definition.isEllipsoid ? definition.BottomDiameterZ : definition.BottomDiameterX;
float tempTopDiameterX, tempTopDiameterZ;
if (definition.type == CylinderShapeType.Cone)
{
tempTopDiameterX = 0;
tempTopDiameterZ = 0;
}
else
if (definition.type == CylinderShapeType.Cylinder)
{
tempTopDiameterX = tempBottomDiameterX;
tempTopDiameterZ = tempBottomDiameterZ;
}
else
{
tempTopDiameterX = definition.TopDiameterX;
tempTopDiameterZ = definition.isEllipsoid ? definition.TopDiameterZ : definition.TopDiameterX;
}
topY = (definition.height + definition.bottomOffset);
bottomY = definition.bottomOffset;
var rotate = Quaternion.AngleAxis(definition.rotation, Vector3.up);
topXVector = rotate * Vector3.right * tempTopDiameterX * 0.5f;
topZVector = rotate * Vector3.forward * tempTopDiameterZ * 0.5f;
bottomXVector = rotate * Vector3.right * tempBottomDiameterX * 0.5f;
bottomZVector = rotate * Vector3.forward * tempBottomDiameterZ * 0.5f;
normal = Vector3.up;
if (topY < bottomY)
{
normal = -Vector3.up;
}
else
{
normal = Vector3.up;
}
topPoint = normal * topY;
bottomPoint = normal * bottomY;
// Render vertical horizon of cylinder
// TODO: make this work with math instead of "finding" it
Vector3 bottomPointA, topPointA;
Vector3 bottomPointB, topPointB;
var camera = UnityEngine.Camera.current;
var cameraTransform = camera.transform;
var cameraPosition = handles.inverseMatrix.MultiplyPoint(cameraTransform.position);
const float degreeStep = 5;
var pointA = fullTopCircleHandle.GetPointAtDegree(360 - degreeStep);
var pointB = fullBottomCircleHandle.GetPointAtDegree(360 - degreeStep);
var camOrtho = camera.orthographic;
var camForward = handles.inverseMatrix.MultiplyVector(cameraTransform.forward).normalized;
var camDir = camOrtho ? camForward : (pointA - cameraPosition).normalized;
var delta = (pointA - pointB).normalized;
var normal3 = -Vector3.Cross(delta, Vector3.Cross((pointB - bottomPoint).normalized, delta)).normalized;
var prevDot = Vector3.Dot(normal3, camDir) < 0;
bool renderHorizon = false;
//*
bottomPointA = Vector3.zero;
topPointA = Vector3.zero;
bottomPointB = Vector3.zero;
topPointB = Vector3.zero;
var lineCount = 0;
for (float degree = 0; degree < 360; degree += degreeStep)
{
pointA = fullTopCircleHandle.GetPointAtDegree(degree);
pointB = fullBottomCircleHandle.GetPointAtDegree(degree);
delta = (pointA - pointB).normalized;
normal3 = -Vector3.Cross(delta, Vector3.Cross((pointB - bottomPoint).normalized, delta)).normalized;
camDir = camOrtho ? camForward : (pointB - cameraPosition).normalized;
var currDot = Vector3.Dot(normal3, camDir) < 0;
if (prevDot != currDot)
{
lineCount++;
if (lineCount == 1)
{
topPointA = pointA;
bottomPointA = pointB;
}
else
//if (lineCount == 2)
{
topPointB = pointA;
bottomPointB = pointB;
renderHorizon = true;
break;
}
}
prevDot = currDot;
}
#if false
{
var pointC = (Vector3.right * (definition.topDiameterX * 0.5f)) + (Vector3.up * (definition.height + definition.bottomOffset));
var pointD = (Vector3.right * (definition.bottomDiameterX * 0.5f)) + (Vector3.up * definition.bottomOffset);
//var deltar = (pointC - pointD).normalized;
//var normala = -Vector3.Cross(Vector3.forward, deltar).normalized;
var DT = (cameraPosition - topPoint);
var DB = (cameraPosition - bottomPoint);
var DmT = DT.magnitude;
var DmB = DB.magnitude;
//var Dv = D / Dm;
var RmT = definition.topDiameterX * 0.5f;
var RmB = definition.bottomDiameterX * 0.5f;
var cosAT = RmT / DmT;
var cosAB = RmB / DmB;
var AT = Mathf.Acos(cosAT) * Mathf.Rad2Deg;
var AB = Mathf.Acos(cosAB) * Mathf.Rad2Deg;
var RvT = (Quaternion.AngleAxis(AT, Vector3.up) * DT).normalized;
var RvB = (Quaternion.AngleAxis(AB, Vector3.up) * DB).normalized;
//var R = Rv * Rm;
var angleT = Vector3.SignedAngle(Vector3.right, RvT, Vector3.up);
var angleB = Vector3.SignedAngle(Vector3.right, RvB, Vector3.up);
var arotationT = Quaternion.AngleAxis(angleT, Vector3.up);
var arotationB = Quaternion.AngleAxis(angleB, Vector3.up);
var ptA = arotationT * pointC;
var ptB = arotationB * pointD;
var prevCol = handles.color;
handles.color = UnityEngine.Color.red;
handles.DrawLine(bottomPoint, bottomPoint + Vector3.right);
//handles.DrawLine(bottomPoint, bottomPoint + Vector3.forward);
//handles.DrawLine(bottomPoint, bottomPoint + normala);
//handles.DrawLine(bottomPoint, bottomPoint + deltar);
//handles.DrawLine(bottomPoint, bottomPoint + R);
handles.DrawLine(bottomPoint, bottomPoint + RvT);
handles.DrawLine(bottomPoint, bottomPoint + RvB);
//handles.DrawLine(bottomPoint, bottomPoint + desired);
handles.DrawLine(ptA, ptB);
handles.color = prevCol;
}
#endif
/*/
* if (camera.orthographic)
* {
* {
* var radius = definition.bottomDiameterX * 0.5f;
* var center = bottomPoint;
* bottomPointA = center + (cameraTransform.right * radius);
* bottomPointB = center - (cameraTransform.right * radius);
* }
* {
* var radius = definition.topDiameterX * 0.5f;
* var center = topPoint;
* topPointA = center + (cameraTransform.right * radius);
* topPointB = center - (cameraTransform.right * radius);
* }
* } else
* {
* var handleMatrix = handles.matrix;
* renderHorizon = GeometryMath.FindCircleHorizon(handleMatrix, definition.bottomDiameterX, bottomPoint, -normal, out bottomPointB, out bottomPointA);
* renderHorizon = GeometryMath.FindCircleHorizon(handleMatrix, definition.topDiameterX, topPoint, normal, out topPointA, out topPointB) && renderHorizon;
*
* if (renderHorizon && definition.bottomDiameterX != definition.topDiameterX)
* {
* renderHorizon = !(GeometryMath.PointInCameraCircle(handleMatrix, bottomPointA, definition.topDiameterX, topPoint, normal) ||
* GeometryMath.PointInCameraCircle(handleMatrix, topPointA, definition.bottomDiameterX, bottomPoint, -normal) ||
* GeometryMath.PointInCameraCircle(handleMatrix, bottomPointB, definition.topDiameterX, topPoint, normal) ||
* GeometryMath.PointInCameraCircle(handleMatrix, topPointB, definition.bottomDiameterX, bottomPoint, -normal));
* }
* }
* //*/
if (!renderHorizon)
{
bottomPointA = Vector3.zero;
topPointA = Vector3.zero;
bottomPointB = Vector3.zero;
topPointB = Vector3.zero;
}
verticalHandle1.From = bottomPointA;
verticalHandle1.To = topPointA;
verticalHandle2.From = bottomPointB;
verticalHandle2.To = topPointB;
fullTopCircleHandle.Center = topPoint;
fullBottomCircleHandle.Center = bottomPoint;
fullTopCircleHandle.DiameterX = tempTopDiameterX;
fullTopCircleHandle.DiameterZ = tempTopDiameterZ;
fullBottomCircleHandle.DiameterX = tempBottomDiameterX;
fullBottomCircleHandle.DiameterZ = tempBottomDiameterZ;
topHandle.Origin = topPoint;
bottomHandle.Origin = bottomPoint;
fullTopCircleHandle.Normal = normal;
fullBottomCircleHandle.Normal = -normal;
topHandle.Normal = normal;
bottomHandle.Normal = -normal;
if (definition.isEllipsoid)
{
if (bottomRadiusHandles == null || bottomRadiusHandles.Length != 4)
{
bottomRadiusHandles = new IChiselEllipsoidHandle[]
{
handles.CreateEllipsoidHandle(Vector3.zero, -normal, 0, 0, startAngle: +45f, angles: 90), // left
handles.CreateEllipsoidHandle(Vector3.zero, -normal, 0, 0, startAngle: +45f + 180f, angles: 90), // right
handles.CreateEllipsoidHandle(Vector3.zero, -normal, 0, 0, startAngle: -45f, angles: 90), // forward
handles.CreateEllipsoidHandle(Vector3.zero, -normal, 0, 0, startAngle: -45f + 180f, angles: 90), // back
};
}
if (topRadiusHandles == null || topRadiusHandles.Length != 4)
{
topRadiusHandles = new IChiselEllipsoidHandle[]
{
handles.CreateEllipsoidHandle(Vector3.zero, normal, 0, 0, startAngle: +45f, angles: 90), // left
handles.CreateEllipsoidHandle(Vector3.zero, normal, 0, 0, startAngle: +45f + 180f, angles: 90), // right
handles.CreateEllipsoidHandle(Vector3.zero, normal, 0, 0, startAngle: -45f, angles: 90), // forward
handles.CreateEllipsoidHandle(Vector3.zero, normal, 0, 0, startAngle: -45f + 180f, angles: 90), // back
};
}
for (int i = 0; i < bottomRadiusHandles.Length; i++)
{
bottomRadiusHandles[i].Center = bottomPoint;
bottomRadiusHandles[i].Normal = -normal;
bottomRadiusHandles[i].DiameterX = tempBottomDiameterX;
bottomRadiusHandles[i].DiameterZ = tempBottomDiameterZ;
bottomRadiusHandles[i].Rotation = definition.rotation;
}
for (int i = 0; i < topRadiusHandles.Length; i++)
{
topRadiusHandles[i].Center = topPoint;
topRadiusHandles[i].Normal = normal;
topRadiusHandles[i].DiameterX = tempTopDiameterX;
topRadiusHandles[i].DiameterZ = tempTopDiameterZ;
topRadiusHandles[i].Rotation = definition.rotation;
}
if (bottomHandles == null || bottomHandles.Length != 4)
{
bottomHandles = new IChiselHandle[] { bottomHandle, bottomRadiusHandles[0], bottomRadiusHandles[1], bottomRadiusHandles[2], bottomRadiusHandles[3] }
}
;
if (definition.type != CylinderShapeType.Cone)
{
if (topHandles == null || topHandles.Length != 5)
{
topHandles = new IChiselHandle[] { topHandle, topRadiusHandles[0], topRadiusHandles[1], topRadiusHandles[2], topRadiusHandles[3] }
}
;
}
else
{
if (topHandles == null || topHandles.Length != 1)
{
topHandles = new IChiselHandle[] { topHandle }
}
;
}
}
else
{
if (bottomRadiusHandles == null || bottomRadiusHandles.Length != 1)
{
bottomRadiusHandles = new IChiselEllipsoidHandle[] { fullBottomCircleHandle }
}
;
if (topRadiusHandles == null || topRadiusHandles.Length != 1)
{
topRadiusHandles = new IChiselEllipsoidHandle[] { fullTopCircleHandle }
}
;
if (bottomHandles == null || bottomHandles.Length != 2)
{
bottomHandles = new IChiselHandle[] { bottomHandle, bottomRadiusHandles[0] }
}
;
if (definition.type != CylinderShapeType.Cone)
{
if (topHandles == null || topHandles.Length != 2)
{
topHandles = new IChiselHandle[] { topHandle, topRadiusHandles[0] }
}
;
}
else
{
if (topHandles == null || topHandles.Length != 1)
{
topHandles = new IChiselHandle[] { topHandle }
}
;
}
}
}
}