public Vector3 calculateVelocity()
{
resultVector = Vector3.zero;
oldfolowingVector = folowingVector;
folowingVector = targetTransform.position - transform.position;
deltaVector = folowingVector - oldfolowingVector;
deltaVector /= Mathf.Pow((targetTransform.position - transform.position).magnitude, deltaVectorOverDistancePow);
deltaVector *= deltaMovementWeight;
collisionAvoidanceVector = Vector3.zero;
nearestSolidsCount = 0;
nearestSolids = Physics.OverlapSphere(transform.position, solidsSearchRadius, collisionAvoidanceMask);
foreach (var solid in nearestSolids)
{
if (solid != GetComponent <Collider>())
{
if (debug)
{
linesDrawer.pushLine(transform.position, solid.transform.position, Color.cyan);
Debug.DrawLine(transform.position, solid.transform.position, Color.cyan);
}
RaycastHit[] rHits;
rHits = Physics.RaycastAll(transform.position, solid.transform.position - transform.position, Vector3.Distance(solid.transform.position, transform.position));
for (int i = 0; i < rHits.Length; i++)
{
if (rHits[i].transform.parent != transform && rHits[i].transform != transform)
{
Vector3 sepColAvoidanceV = (transform.position - rHits[i].point) / Mathf.Pow((transform.position - rHits[i].point).magnitude, collAvoidOverDistPow);
sepColAvoidanceV *= colAvoidanceWeight;
collisionAvoidanceVector += sepColAvoidanceV;
nearestSolidsCount++;
}
}
}
}
// hits = Physics.RaycastAll(transform.position, folowingVector ,Vector3.Distance(targetTransform.position,transform.position));
// for(int i = 0; i < hits.Length; i++)
// {
// if(hits[i].transform.parent != transform)
// {
// if(hits[i].transform != transform)
// {
// if(hits[i].transform.parent != targetTransform)
// {
// if(hits[i].transform != targetTransform)
// {
// //folowingVector = Vector3.zero;//if hits something, we will not fly in this direction
// resultVector = folowingVector;
// break;
// }
// }
// }
// }
// if( hits[i].transform == targetTransform)
// {
// resultVector = folowingVector;
// break;//leave following vector as is
// }
// }
resultVector = collisionAvoidanceVector + folowingVector + deltaVector - rb.velocity;
if (debug)
{
linesDrawer.pushLine(transform.position, transform.position - rb.velocity, Color.yellow);
Debug.DrawLine(transform.position, transform.position - rb.velocity, Color.yellow);
linesDrawer.pushLine(transform.position, transform.position + deltaVector, Color.red);
Debug.DrawLine(transform.position, transform.position + deltaVector, Color.red);
linesDrawer.pushLine(transform.position, transform.position + collisionAvoidanceVector, Color.green);
Debug.DrawLine(transform.position, transform.position + collisionAvoidanceVector, Color.green);
linesDrawer.pushLine(transform.position, transform.position + resultVector, Color.white);
Debug.DrawLine(transform.position, transform.position + resultVector, Color.white);
linesDrawer.pushLine(transform.position, transform.position + folowingVector, Color.blue);
Debug.DrawLine(transform.position, transform.position + folowingVector, Color.blue);
}
if (resultVector.magnitude < doForceTreshold)
{
return(collisionAvoidanceVector + deltaVector - rb.velocity);
}
else
{
// if(debug)
// {
// linesDrawer.pushLine(transform.position, transform.position + folowingVector, Color.blue);
// Debug.DrawLine(transform.position, transform.position + folowingVector, Color.blue);
// }
return(resultVector);
}
}
// Update is called once per frame
void Update()
{
passedTime += Time.deltaTime;
if (passedTime > updateInterval)
{
for (int i = 1; i < dotsCount; i++)
{
values[i - 1] = values[i];
}
if (smoothGraph)
{
neededSpeed = currentValue - oldValue;
if (speed > neededSpeed)
{
speed -= dotAccelFactor * passedTime;
}
if (speed < neededSpeed)
{
speed += dotAccelFactor * passedTime;
}
values[dotsCount - 1] = oldValue + speed * smoothFactor * passedTime;
oldValue = values[dotsCount - 1];
}
else
{
values[dotsCount - 1] = currentValue;
}
float topValue = values.Max();
if (topValue < minValue)
{
topValue = minValue;
}
if (!glDrawer)
{
for (int i = 1; i < dotsCount; i++)
{
lr.SetPosition(i, new Vector3(((float)i / dotsCount) * horScale, (values[i] / topValue) * verScale, 0));
}
}
else
{
for (int i = 1; i < dotsCount; i++)
{
linesDrawer.pushLine(
transform.position + (transform.right * (i - 1) * horScale) + (transform.up * (values[i - 1] / topValue) * verScale),
transform.position + (transform.right * i * horScale) + (transform.up * (values[i] / topValue) * verScale),
Color.white);
}
}
passedTime = 0;
}
}
