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); } }