public static Vector2 GetDirection(RVOAgent agent)
{
Vector2 nowVelocity = Simulator.Instance.getAgentVelocity(agent.GetIndex());
if (RVOMath.abs(nowVelocity) < eps)
{
nowVelocity = agent.GetDirection();
}
return(RVOMath.normalize(nowVelocity));
}
public static bool IsVectorNear(Vector2 a, Vector2 b, float range)
{
Vector2 aNormalize = RVOMath.normalize(a);
Vector2 bNormalize = RVOMath.normalize(b);
//Debug.Log(Mathf.Abs(RVOMath.det(aNormalize, bNormalize)));
if (Mathf.Abs(RVOMath.det(aNormalize, bNormalize)) < range && RVOMath.abs(aNormalize - bNormalize) < 1.0f)
{
return(true);
}
return(false);
}
void doStep()
{
/* Create agent ORCA lines. */
for (int i = 0; i < 1; ++i)
{
Vector2 relativePosition = otherPosition_ - position_;
Vector2 relativeVelocity = velocity_ - otherVelocity_;
float distSq = RVOMath.absSq(relativePosition);
float combinedRadius = radius_ + otherRadius_;
float combinedRadiusSq = RVOMath.sqr(combinedRadius);
Line line;
Vector2 u;
if (distSq > combinedRadiusSq)
{
/* No collision. */
Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
float wLengthSq = RVOMath.absSq(w);
float dotProduct1 = w * relativePosition;
//Debug.Log("w:" + w.ToString() + " dotProduct1:" + dotProduct1.ToString());
if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
{
/* Project on cut-off circle. */
float wLength = RVOMath.sqrt(wLengthSq);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else
{
/* Project on legs. */
float leg = RVOMath.sqrt(distSq - combinedRadiusSq);
if (RVOMath.det(relativePosition, w) > 0.0f)
{
/* Project on left leg. */
line.direction = new Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}
else
{
/* Project on right leg. */
line.direction = -new Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}
float dotProduct2 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else
{
/* Collision. Project on cut-off circle of time timeStep. */
float invTimeStep = 1.0f / Simulator.Instance.timeStep_;
/* Vector from cutoff center to relative velocity. */
Vector2 w = relativeVelocity - invTimeStep * relativePosition;
float wLength = RVOMath.abs(w);
Vector2 unitW = w / wLength;
line.direction = new Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
line.point = velocity_ + 0.5f * u;
Debug.Log("u:" + u.ToString() + " Point:" + line.point.ToString() + " Direction:" + line.direction.ToString());
orcaLines_.Add(line);
}
float maxSpeed = 2.0f;
Vector2 prefVelocity = new Vector2(1, 1);
Vector2 newVelocity = new Vector2(0, 0);
linearProgram1(orcaLines_, 0, maxSpeed, prefVelocity, false, ref newVelocity);
//Debug.Log("prefVelocity:" + prefVelocity.ToString() + " newVelocity:" + newVelocity.ToString());
/*
* int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_);
*
* if (lineFail < orcaLines_.Count)
* {
* linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_);
* }
*/
}
void Update()
{
if (isOk && agentIndex != -1)
{
/* dist */
transform.position = RVOWithUnity.Vec2ToVec3(Simulator.Instance.getAgentPosition(agentIndex));
/* turn */
if (RVOMath.abs(Simulator.Instance.getAgentVelocity(agentIndex)) > 0.0f)
{
Vector3 direction = RVOWithUnity.Vec2ToVec3(Simulator.Instance.getAgentVelocity(agentIndex));
float angle = Vector3.Angle(Vector3.right, direction);
if (Vector3.Cross(Vector3.right, direction).y < 0)
{
angle = 360 - angle;
}
transform.rotation = Quaternion.Euler(new Vector3(transform.rotation.x, angle, transform.rotation.z));
}
if (!isStop)
{
/* Velocity */
velocity = Simulator.Instance.getAgentVelocity(agentIndex);
velocityVec3 = RVOWithUnity.Vec2ToVec3(velocity);
speed = RVOMath.abs(velocity);
/* Update Station */
station = pathNodes[nowNode];
Vector2 stationVector2 = RVOWithUnity.Vec3ToVec2(station);
Vector2 transformVector2 = RVOWithUnity.Vec3ToVec2(transform.position);
Vector2 distVector2 = stationVector2 - transformVector2;
Vector2 lastStationVector2 = RVOWithUnity.Vec3ToVec2(pathNodes[nowNode - 1]);
Vector2 lastToNowNormalize = RVOMath.normalize(stationVector2 - lastStationVector2);
Vector2 verticalDistVector2 = new Vector2(-lastToNowNormalize.y_, lastToNowNormalize.x_);
//turnDist = (speed / angularSpeed) / changeStation;
if (RVOWithUnity.isTwoSide(verticalDistVector2, distVector2, lastToNowNormalize))
{
nowNode++;
slowStationNum = Mathf.Max(0, slowStationNum - 1);
//Debug.Log(RVOWithUnity.Vec3ToVec2(pathNodes[nowNode]));
if (nowNode > pathNodes.Count - 1)
{
isStop = true;
prefSpeed = 0.0f;
}
else
{
station = pathNodes[nowNode];
}
if (slowStationNum == 0)
{
prefSpeed = maxSpeed;
}
}
else
{
float slowDist = Mathf.Abs(speed * speed - minSpeedToTurn * minSpeedToTurn) / (acceleration * slowStation * 2);
if (RVOMath.abs(distVector2) >= slowDist && slowStationNum == 0)
{
prefSpeed = maxSpeed;
}
if (RVOMath.abs(distVector2) < slowDist)
{
/* Calc the slowScale */
if (nowNode + 1 > pathNodes.Count - 1)
{
prefSpeed = minSpeedToTurn;
turnDist = 0.1f;
}
else
{
int nextTurnNode = nowNode + 1;
Vector2 nextStation = RVOWithUnity.Vec3ToVec2(pathNodes[nextTurnNode]);
Vector2 lastStation = stationVector2;
Vector2 nextDirection = nextStation - lastStation;
float turnAngleDist = Vector3.Angle(velocityVec3, RVOWithUnity.Vec2ToVec3(nextDirection));
if (nowNode + 1 <= pathNodes.Count - 1 && slowStationNum == 0)
{
slowStationNum++;
float turnAngle = turnAngleDist;
//if (gameObject.name == "boat")
// Debug.Log(lastStation + "/|/" + nextStation + "/|/" + speed * ignoreStepFactor);
while (RVOMath.abs(nextDirection) < speed * ignoreStepFactor && nextTurnNode + 1 <= pathNodes.Count - 1)
{
nextTurnNode++;
slowStationNum++;
lastStation = nextStation;
nextStation = RVOWithUnity.Vec3ToVec2(pathNodes[nextTurnNode]);
Vector2 lastDirection = nextDirection;
nextDirection = nextStation - lastStation;
//Debug.Log(nextStation + " " + lastStation);
turnAngle += Vector3.Angle(RVOWithUnity.Vec2ToVec3(lastDirection), RVOWithUnity.Vec2ToVec3(nextDirection));
}
prefSpeed = minSpeedToTurn + (maxSpeed - minSpeedToTurn) * Mathf.Max(turningFactor - turnAngle, turningFactor / 10.0f) / turningFactor;
}
turnDist = Mathf.Tan(turnAngleDist * Mathf.Deg2Rad / 2) * (prefSpeed * prefSpeed / angularSpeed) / 200.0f;
}
}
if (RVOMath.abs(distVector2) < turnDist)
{
nowNode++;
slowStationNum = Mathf.Max(0, slowStationNum - 1);
if (nowNode > pathNodes.Count - 1)
{
isStop = true;
prefSpeed = 0.0f;
}
else
{
station = pathNodes[nowNode];
}
}
}
}
}
}
private void Update()
{
if (inited == false)
{
return;
}
//if ((Time.time - lastTime) > 0.25f)
//{
// lastTime = Time.time;
// for (int i = 0; i < goals.Count; i++)
// {
// goals[i] += dirs[i] * 0.8f;
// Debug.DrawLine(goals[i].ToVec3XZ(),goals[i].ToVec3XZ()+Vector3.up*5f,Color.cyan,0.25f);
// }
//}
if (ReachedGoal() == false)
{
SetPreferredVelocities();
Simulator.Instance.doStep();
}
for (int i = 0; i < agents.Count; i++)
{
vec2 = Simulator.Instance.getAgentPosition(i);
realPos.x = vec2.x();
realPos.z = vec2.y();
vec2 = Simulator.Instance.getAgentVelocity(i);
velocity.x = vec2.x_;
velocity.z = vec2.y_;
agentPos = agents[i].transform.position;
Debug.DrawLine(agentPos + Vector3.up * 1f,
agentPos + velocity.normalized * 2.5f + Vector3.up * 1f,
Color.green, Time.deltaTime);
Debug.DrawLine(realPos, realPos + Vector3.up * 10, Color.red, Time.deltaTime);
agents[i].transform.position = Vector3.Lerp(agentPos, realPos, Time.deltaTime * RVOMath.abs(Simulator.Instance.getAgentVelocity(i)));
}
}
public static Vector2 CalcPrefVelocity(Vector2 agentPosition, Vector2 goalPosisiton, RVOAgent agent)
{
Vector2 prefDistance = goalPosisiton - agentPosition;
if (agent.GetIsStop() == true)
{
return(new Vector2(0f, 0f));
}
if (agent.isPlayer == true)
{
/* Is inertial model used */
Vector2 nowVelocity = Simulator.Instance.getAgentVelocity(agent.GetIndex());
float nowSpeed = RVOMath.abs(nowVelocity);
Vector2 prefVelocity = RVOMath.normalize(prefDistance) * agent.prefSpeed;//nowSpeed -> prefSpeed
Vector2 finalVelocity = new Vector2(0, 0);
if (nowSpeed < agent.minSpeedToTurn)
{
/* Not fast enough to turn */
Vector2 advanceDirection = GetDirection(agent);
if (agent.prefSpeed > nowSpeed) //Speed up fitting
{
finalVelocity = nowVelocity + advanceDirection * agent.acceleration;
}
else //Speed down fitting
{
if (nowSpeed - agent.prefSpeed < agent.acceleration)
{
finalVelocity = prefVelocity;
}
else
{
finalVelocity = nowVelocity - advanceDirection * agent.acceleration;
}
}
}
else
{
/* turn */
float direction = RVOMath.det(nowVelocity, prefVelocity);
if (direction > eps) //left
{
Vector2 rightVerticalVelocity = new Vector2(-nowVelocity.y_, nowVelocity.x_);
rightVerticalVelocity = RVOMath.normalize(rightVerticalVelocity) * agent.speed * agent.angularSpeed;
if (RVOMath.det(nowVelocity + rightVerticalVelocity, prefVelocity) < eps) //success
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
else
{
finalVelocity = RVOMath.normalize(nowVelocity + rightVerticalVelocity) * nowSpeed;
}
}
if (direction >= -eps && direction <= eps) //same direction
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
if (direction < -eps) //right
{
Vector2 leftVerticalVelocity = new Vector2(nowVelocity.y_, -nowVelocity.x_);
leftVerticalVelocity = RVOMath.normalize(leftVerticalVelocity) * agent.speed * agent.angularSpeed;
if (RVOMath.det(nowVelocity + leftVerticalVelocity, prefVelocity) > -eps) //success
{
finalVelocity = RVOMath.normalize(prefVelocity) * nowSpeed;
}
else
{
finalVelocity = RVOMath.normalize(nowVelocity + leftVerticalVelocity) * nowSpeed;
}
}
//Debug.Log("!!!" + RVOMath.abs(finalVelocity).ToString());
/* acc and slow */
if (agent.prefSpeed - nowSpeed > eps) //acc
{
if (IsVectorNear(nowVelocity, prefVelocity, 0.05f))
{
finalVelocity = finalVelocity + RVOMath.normalize(finalVelocity) * Mathf.Min(agent.acceleration, agent.prefSpeed - nowSpeed);
}
}
else //slow
{
finalVelocity = finalVelocity - RVOMath.normalize(finalVelocity) * Mathf.Min(agent.acceleration, nowSpeed - agent.prefSpeed);
}
// Debug.Log("!!!" + RVOMath.abs(finalVelocity).ToString());
}
Vector2 ditherVelocity = GetDitherVelocity(ditherSize);
if (RVOMath.abs(finalVelocity) < eps)
{
return(finalVelocity);
}
else
{
return(finalVelocity + ditherVelocity);
}
}
else
{
Vector2 ditherVelocity = GetDitherVelocity(ditherSize);
Vector2 prefVelocity = RVOMath.normalize(prefDistance);
return(prefVelocity * agent.prefSpeed + ditherVelocity);
}
}
