public override void CalculateFinalVelocity(FP deltaTime, bool bSkipBoids = false) { FP fDtSecond = deltaTime; bool bReachedTarget = false; //(_targetPos - _behaviour.position).sqrMagnitude < GridMap.GetNodeSize(); if (!bReachedTarget) { TSVector pos = _behaviour.position; // ((GridMap)(_map)).GetGridFloatCoord(_behaviour.position); TSVector realAcc = TSVector.zero; //real acceleration if (PathFindingManager.c_useAvoidUnit) { int preBoidsType = _activeBoids; _activeBoids = (int)EBoidsActiveType.seperation; // (int)EBoidsActiveType.collisionAvoidance | TSVector velocity = _behaviour.velocity; TSVector desiredV; FP dFactor; FP maxTime; FP obstacleMaxTime = FP.Zero; TSVector dA = DesiredAcc(_targetPos, out desiredV, out dFactor, out maxTime); //_targetPos obstacleMaxTime = FP.One; // TSMath.Min(maxTime, FP.One); maxTime = FP.One; // TSMath.Min(maxTime, FP.One); if (dA != TSVector.zero || desiredV != TSVector.zero) { //check dA dir is clear or not TSVector tryV = desiredV;//.normalized * _behaviour.maxSpeed; bool bStatic = false; FP ttcTry = -1; bool isCollidering = false; TSVector try_a = TSVector.zero; { try_a = CustomMath.TSVec2ToVec(ForceBasedAgent.computeForces (_behaviour, null, tryV, false, maxTime, obstacleMaxTime , out bStatic, out ttcTry, ref isCollidering, true)) * _behaviour.baseData.invMass; } bool dDirClear = ttcTry < 0;// desired direction is clear or not bool isRealAccZero = true; bool isTminStaticAgent = false; bool isNearStaticAgent = false; TSVector sepA = TSVector.zero; if (!dDirClear) // { isTminStaticAgent = false; FP ttc = -1; bool newMoveing = velocity == TSVector.zero; if (newMoveing) { int count = _behaviour.neighbours.Count; realAcc = CustomMath.perpendicular(desiredV); } else { realAcc = CustomMath.TSVec2ToVec(ForceBasedAgent.computeForces(_behaviour, null, velocity, false, 1, 1, out isTminStaticAgent, out ttc, ref isCollidering, true)) * _behaviour.baseData.invMass; if (ttc > 0 && (realAcc * _behaviour.baseData.mass).sqrMagnitude > _behaviour.baseData.maxForceSqr) { realAcc = _behaviour.baseData.maxForce * _behaviour.baseData.invMass * realAcc.normalized; } } TSVector preA = realAcc; isRealAccZero = realAcc == TSVector.zero; if (!isRealAccZero && isTminStaticAgent) //current dir is not clear,blocked by static object { //current direction is not clear, //then turn to perpendicular direction of avoid direction if (ttc > 0) { realAcc = CustomMath.perpendicular(realAcc); } bool changeA = false; if (realAcc * _behaviour.velocity < 0) // * desiredV { TSVector pos2 = _behaviour.position + realAcc.normalized * GridMap.blockDirDst; bool hashNeighbour = _behaviour.baseData.pfm.HasNeighboursBetween2Point(_behaviour.proxyId, _behaviour.position , pos2, _behaviour.pathManager._queryStack); if (!hashNeighbour) { changeA = true; } } if (changeA) { #if UNITY_EDITOR && !MULTI_THREAD if (PathFindingManager.DEBUG) { UnityEngine.Debug.Log("change real acc dir"); } #endif realAcc = TSVector.zero - realAcc; } } isNearStaticAgent = ttc < FP.EN1 * 2 && isTminStaticAgent; // otherwise,take boids force into consideration if (isCollidering) { sepA = ApplyForce(deltaTime, TSVector.zero, velocity, pos, _behaviour.neighbours, bSkipBoids, false);// realAcc = realAcc + sepA; } isRealAccZero = realAcc == TSVector.zero; } if (isRealAccZero) //current diretion is clear { // if (!dDirClear) //disired diretion is not clear { //check perpendicular of velocity is clear or not,1,5 if (_behaviour.velocity == TSVector.zero) { _behaviour.preVelocity = CustomMath.perpendicular(desiredV); velocity = _behaviour.velocity; } tryV = (desiredV * _behaviour.invMaxSpeed + _behaviour.preVelocity.normalized * _currentVelFactor).normalized * _behaviour.maxSpeed; //CustomMath.perpendicular(velocity); // tryV = (TSVector.Dot(tryV, desiredV) >= 0 ? tryV : (TSVector.zero - tryV)).normalized * _behaviour.maxSpeed;// velocity.magnitude bStatic = false; ttcTry = -1; try_a = CustomMath.TSVec2ToVec(ForceBasedAgent.computeForces (_behaviour, null, tryV, false, 1, 1, out bStatic, out ttcTry, ref isCollidering, true)) * _behaviour.baseData.invMass; if (ttcTry > 0) //not clear,use pre velocity ttcTry < FP.EN1*2 && { _currentVelFactor = Max_Vel_Factor; _behaviour.preVelocity = velocity.normalized * _behaviour.maxSpeed; #if UNITY_EDITOR && !MULTI_THREAD if (PathFindingManager.DEBUG) { UnityEngine.Debug.Log("use pre velocity :" + CustomMath.TsVecToVector3(_behaviour.preVelocity)); TSVector dirPos = _behaviour.position + _behaviour.preVelocity.normalized * 2; UnityEngine.Debug.DrawLine(CustomMath.TsVecToVector3(_behaviour.position), CustomMath.TsVecToVector3(dirPos), UnityEngine.Color.magenta, 0.1f); } #endif // _nextCalDesiredVelTime = _behaviour.baseData.pfm.CurTime //+ PathFindingAgentBehaviour.C_DESIRED_DELTATIME * 2; return; } else // test dir is clear then turn to test dir { _currentVelFactor *= 7 * FP.EN1; _behaviour.preVelocity = tryV; // #if UNITY_EDITOR && !MULTI_THREAD if (PathFindingManager.DEBUG) { UnityEngine.Debug.Log("turn to test dir:" + CustomMath.TsVecToVector3(_behaviour.preVelocity)); TSVector dirPos = _behaviour.position + _behaviour.preVelocity.normalized * 2; UnityEngine.Debug.DrawLine(CustomMath.TsVecToVector3(_behaviour.position), CustomMath.TsVecToVector3(dirPos), UnityEngine.Color.cyan, 0.1f); } #endif //dA = tryV; return; } } else //turn to desired direction { _behaviour.preVelocity = CustomMath.Normalize(desiredV) * _behaviour.maxSpeed; #if UNITY_EDITOR && !MULTI_THREAD if (PathFindingManager.DEBUG) { UnityEngine.Debug.Log("turn to desired direction:" + CustomMath.TsVecToVector3(_behaviour.preVelocity)); } #endif return; } } //current diretion is not clear,go on //has static block nearby,try to find clear dir if (isNearStaticAgent && !isCollidering) //8 directions isTminStaticAgent { TSVector dNormlDir = desiredV * _behaviour.invMaxSpeed; TSVector perpendicular1 = CustomMath.perpendicular(dNormlDir); TSVector perpendicular2 = TSVector.zero - perpendicular1; //45 from disired direction TSVector test45Dir1 = (perpendicular1 + dNormlDir).normalized; TSVector minAngleTestDir = perpendicular1; FP maxDot = FP.MinValue; bool bOk = false; for (int i = 0; i < 7; i++) { ECheckDirStatus status = CheckDir(i, dNormlDir, perpendicular1, perpendicular2, test45Dir1 , ref maxDot, ref minAngleTestDir, true); if (status == ECheckDirStatus.ok) { bOk = true; break; } else if (status == ECheckDirStatus.delay) { bOk = true; } } if (maxDot != FP.MinValue) //status = ECheckDirStatus.delay { _behaviour.preVelocity = minAngleTestDir * _behaviour.maxSpeed; if (minAngleTestDir == perpendicular1) //?????????? { _preTestOKVec = perpendicular1; } else if (minAngleTestDir == perpendicular2) { _preTestOKVec = perpendicular2; } } if (bOk) { return; } } TSVector validA = dA + realAcc; // FP accFactor = 1; FP validTime = deltaTime; bool ignoreDA = (!isRealAccZero) && (isTminStaticAgent || sepA != TSVector.zero); //not calculating desired velocity and (near static block or boids force influences ) if (ignoreDA) // && _nextCalDesiredVelTime > _behaviour.baseData.pfm.CurTime { validA = realAcc; // accFactor = 1; #if UNITY_EDITOR && !MULTI_THREAD if (PathFindingManager.DEBUG) { UnityEngine.Debug.Log("validA:a:" + CustomMath.TsVecToVector3(realAcc) + ",isTminStaticAgent:" + isTminStaticAgent + ",sepA:" + CustomMath.TsVecToVector3(sepA) + ",isNearStaticAgent" + isNearStaticAgent); } #endif } else { #if UNITY_EDITOR && !MULTI_THREAD if (PathFindingManager.DEBUG) { UnityEngine.Debug.Log("calculating desired velocity or not static blocked"); } #endif } if (((validA) * _behaviour.baseData.mass).sqrMagnitude > _behaviour.baseData.maxForceSqr) { validA = validA.normalized * _behaviour.baseData.maxForce * _behaviour.baseData.invMass; } _behaviour.preVelocity = velocity + (validA) * validTime; // #if UNITY_EDITOR && !MULTI_THREAD if (PathFindingManager.DEBUG) { TSVector forcePos = _behaviour.position + validA.normalized * 2; UnityEngine.Debug.DrawLine(CustomMath.TsVecToVector3(_behaviour.position), CustomMath.TsVecToVector3(forcePos), UnityEngine.Color.yellow, 0.1f); } #endif if (_behaviour.preVelocity.sqrMagnitude > _behaviour.maxSpeed * _behaviour.maxSpeed)//|| TSVector.Dot(_behaviour.velocity, a) > 0 { _behaviour.preVelocity = CustomMath.Normalize(_behaviour.preVelocity) * _behaviour.maxSpeed; } #if UNITY_EDITOR && !MULTI_THREAD if (PathFindingManager.DEBUG) { UnityEngine.Debug.Log(" _behaviour.preVelocity:" + CustomMath.TsVecToVector3(_behaviour.preVelocity)); } #endif // _preForceAcc = a; } else if (desiredV == TSVector.zero) { _behaviour.preVelocity = TSVector.zero; } _activeBoids = preBoidsType; } } }
public ECheckDirStatus CheckDir(int dirIdx, TSVector dNormlDir, TSVector perpendicular1, TSVector perpendicular2, TSVector test45Dir1, ref FP maxDot, ref TSVector minAngleTestDir, bool bIgnoreObstacle) { FP dirDst = GridMap.blockDirDst; TSVector testDir = TSVector.zero; bool bExtraCheck = true; ECheckDirStatus status = ECheckDirStatus.fail; switch (dirIdx) { case 0: //45 from disired direction testDir = test45Dir1; //45 to disired direction break; case 1: //45 to disired direction TSVector test45Dir2 = CustomMath.perpendicular(test45Dir1); testDir = test45Dir2 * dNormlDir > 0 ? test45Dir2 : (TSVector.zero - test45Dir2); break; case 2: //perpendicular direction 1 testDir = perpendicular1; //--- // | handle this situation ,>5 degree from previous test ok direction //--- bExtraCheck = _preTestOKVec * perpendicular2 < 996 * FP.EN3; break; case 3: //perpendicular direction 2 testDir = perpendicular2; bExtraCheck = _preTestOKVec * perpendicular1 < 996 * FP.EN3; break; case 4: //135 from disired direction 1 testDir = TSVector.zero - test45Dir1; break; case 5: //135 to disired direction testDir = CustomMath.perpendicular(test45Dir1); testDir = testDir * dNormlDir < 0 ? testDir : (TSVector.zero - testDir); break; case 6: //opposite direction testDir = TSVector.zero - dNormlDir; break; } TSVector pos2 = _behaviour.position + testDir * dirDst; if (bExtraCheck && !IsBlockedBetween2Point(_behaviour.position, pos2 , _behaviour.pathManager._queryStack, bIgnoreObstacle)) { bool bOk = dirIdx < 2; if (dirIdx >= 2) { FP dot = _behaviour.velocity * testDir; if (dot < 0) { if (dot > maxDot) { maxDot = dot; minAngleTestDir = testDir; } status = ECheckDirStatus.delay; } else { status = ECheckDirStatus.ok; } } else { status = ECheckDirStatus.ok; } if (status == ECheckDirStatus.ok) { _behaviour.preVelocity = testDir * _behaviour.maxSpeed; return(status); } } return(status); }