public override void OnSyncedStart() { FP a = 3.7897495f; for (int i = 0; i < 100; i++) { a = Mathf.Sin(a.AsFloat() + a.AsFloat()); Debug.Log("a: " + a); } }
private void Update() { transform.localRotation = TSQuaternion.Euler(TSVector.up * this.wheelAngle).ToQuaternion(); // tsTransform.localRotation = TSQuaternion.Euler(TSVector.up * this.wheelAngle); Debug.DrawRay(transform.position, -transform.up * (springLength.AsFloat() + wheelRadius), Color.red); }
private void OnDrawGizmosSelected() { Gizmos.color = Color.blue; Gizmos.DrawWireSphere(transform.position, m_Threshold.AsFloat()); if (m_RespawnPoints != null) { for (int respawnPointIndex = 0; respawnPointIndex < m_RespawnPoints.Length; ++respawnPointIndex) { RespawnPoint respawnPoint = m_RespawnPoints[respawnPointIndex]; if (respawnPoint == null) { continue; } TSVector2 normalizedDir = respawnPoint.forceDirection.normalized; // Draw main direction. if (respawnPoint.transform != null) { Gizmos.color = Color.yellow; Vector3 from = respawnPoint.transform.position.ToVector(); Vector3 to = from + new Vector3(normalizedDir.x.AsFloat(), normalizedDir.y.AsFloat(), 0f); Gizmos.DrawLine(from, to); } if (MathFP.Abs(respawnPoint.errorAngle) > FP.Zero) { // Draw error range. { Gizmos.color = Color.red; TSVector2 normalizedDirLeft = normalizedDir.Rotate(-respawnPoint.errorAngle); Vector3 from = respawnPoint.transform.position.ToVector(); Vector3 to = from + new Vector3(normalizedDirLeft.x.AsFloat(), normalizedDirLeft.y.AsFloat(), 0f); Gizmos.DrawLine(from, to); } { Gizmos.color = Color.red; TSVector2 normalizedDirRight = normalizedDir.Rotate(respawnPoint.errorAngle); Vector3 from = respawnPoint.transform.position.ToVector(); Vector3 to = from + new Vector3(normalizedDirRight.x.AsFloat(), normalizedDirRight.y.AsFloat(), 0f); Gizmos.DrawLine(from, to); } } } } }
void OnDrawGizmos() { if (!m_DrawGizmos) { return; } Gizmos.color = Color.red; Gizmos.DrawWireSphere(transform.position, m_Radius.AsFloat()); }
public override void OnSyncedUpdate() { _movX = TrueSyncInput.GetFP(0); _movY = TrueSyncInput.GetFP(1); _rotX = TrueSyncInput.GetFP(2); _rotY = TrueSyncInput.GetFP(3); Debug.Log("move:" + new Vector2(_movX.AsFloat(), _movY.AsFloat()) + "rot:" + new Vector2(_rotX.AsFloat(), _rotY.AsFloat())); // 移动 Move(); // 开火 Fire(); }
static UnityEngine.Color Rainbow(FP v) { UnityEngine.Color c = new UnityEngine.Color(v.AsFloat(), 0, 0); if (c.r > 1) { c.g = c.r - 1; c.r = 1; } if (c.g > 1) { c.b = c.g - 1; c.g = 1; } return(c); }
public void Loop(FP frameTime, TSVector position, TSVector velocity) { if (!enabled || !gameObject.activeSelf) { return; } if (unit != null) { // attacRange = unit._attackRange.AsFloat(); _agentType = unit._agentType; _velocity = CustomMath.TsVecToVector3(unit.velocity); _preVelocity = CustomMath.TsVecToVector3(unit.preVelocity); _offsetTime = unit._frameOffset; _id = unit.Id; targetPos = CustomMath.TsVecToVector3(unit.targetPos); } // Debug.Log(unit.Id+ ",speed:"+velocity.magnitude); FP trueAtkRange = (attackRange + unit.colliderRadius);//warning,_listEnemyAgents[0].colliderRadius FP atkRangeSqr = trueAtkRange * trueAtkRange; if (unit.targetPos != TSVector.MaxValue) { FP dstSqr = (unit.targetPos - unit.position).sqrMagnitude; if (unit.AgentType != EAgentType.none) { unit.stopMoving = dstSqr <= atkRangeSqr; } } gameObject.transform.position = CustomMath.TsVecToVector3(position); if (Time.realtimeSinceStartup - fTime > Time.deltaTime * 2 && fTime > 0) { // Debug.LogError("diff time:"+ (Time.realtimeSinceStartup - fTime) ); } fTime = Time.realtimeSinceStartup; if (velocity.magnitude / frameTime > 0.01f) //todo { //transform.forward = CustomMath.TsVecToVector3(velocity);//todo var rot = transform.rotation; var targetRot = Quaternion.LookRotation(CustomMath.TsVecToVector3(velocity), transform.up); const float RotationSpeed = 5; transform.rotation = Quaternion.Slerp(rot, targetRot, frameTime.AsFloat() * RotationSpeed); } }
protected float GetVehicleSpeed(GameObject i_Go) { if (i_Go == null) { return(0f); } TSRigidBody2D rigidbody2d = i_Go.GetComponent <TSRigidBody2D>(); if (rigidbody2d != null) { TSVector2 tsVelocity = rigidbody2d.velocity; FP tsSpeed = tsVelocity.magnitude; float speed = tsSpeed.AsFloat(); return(speed); } return(0f); }
/** Creates a VO for avoiding another agent. * \param center The position of the other agent relative to this agent. * \param offset Offset of the velocity obstacle. For example to account for the agents' relative velocities. * \param radius Combined radius of the two agents (radius1 + radius2). * \param inverseDt 1 divided by the local avoidance time horizon (e.g avoid agents that we will hit within the next 2 seconds). * \param inverseDeltaTime 1 divided by the time step length. */ public VO(TSVector2 center, TSVector2 offset, FP radius, FP inverseDt, FP inverseDeltaTime) { // Adjusted so that a parameter weightFactor of 1 will be the default ("natural") weight factor this.weightFactor = 1; weightBonus = 0; //this.radius = radius; TSVector2 globalCenter; circleCenter = center * inverseDt + offset; FP tmp = Sqr(center.LengthSquared() / (radius * radius));//exp(-tmp), // tmp = 1 +tmp+tmp*tmp/2+tmp*tmp*tmp/6; //simple use Taylor's Formula this.weightFactor = 4 * System.Math.Exp(-tmp.AsFloat()) + 1;// 4 /tmp + 1;//exp() // Collision? if (center.magnitude < radius) { colliding = true; // 0.001 is there to make sure lin1.magnitude is not so small that the normalization // below will return TSVector2.zero as that will make the VO invalid and it will be ignored. line1 = center.normalized * (center.magnitude - radius - FP.One / 1000) * 3 / 10 * inverseDeltaTime; dir1 = new TSVector2(line1.y, -line1.x).normalized; line1 += offset; cutoffDir = TSVector2.zero; cutoffLine = TSVector2.zero; dir2 = TSVector2.zero; line2 = TSVector2.zero; this.radius = 0; } else { colliding = false; center *= inverseDt; radius *= inverseDt; globalCenter = center + offset; // 0.001 is there to make sure cutoffDistance is not so small that the normalization // below will return TSVector2.zero as that will make the VO invalid and it will be ignored. var cutoffDistance = center.magnitude - radius + FP.One / 1000; cutoffLine = center.normalized * cutoffDistance; cutoffDir = new TSVector2(-cutoffLine.y, cutoffLine.x).normalized; cutoffLine += offset; FP alpha = TSMath.Atan2(-center.y, -center.x); FP delta = TSMath.Abs(TSMath.Acos(radius / center.magnitude)); this.radius = radius; // Bounding Lines // Point on circle line1 = new TSVector2(TSMath.Cos(alpha + delta), TSMath.Sin(alpha + delta)); // Vector tangent to circle which is the correct line tangent // Note that this vector is normalized dir1 = new TSVector2(line1.y, -line1.x); // Point on circle line2 = new TSVector2(TSMath.Cos(alpha - delta), TSMath.Sin(alpha - delta)); // Vector tangent to circle which is the correct line tangent // Note that this vector is normalized dir2 = new TSVector2(line2.y, -line2.x); line1 = line1 * radius + globalCenter; line2 = line2 * radius + globalCenter; } segmentStart = TSVector2.zero; segmentEnd = TSVector2.zero; segment = false; }
// Update is called once per frame public override void OnSyncedUpdate() { gauge += TrueSyncManager.DeltaTime / cust_speed; if (pending_energy_use) { gauge -= used_energy; } pending_energy_use = false; gauge = (gauge > 10.0f) ? 10.0f : gauge; energy = (FP)TrueSync.TSMath.Floor(gauge); cust_num.GetChild(0).GetComponent <Text>().text = "" + energy; // gauge filling int i = 0; while (i < 10) //increment through each bar; the first 9 children { if (i < energy) // full bars { transform.GetChild(i).GetChild(0).GetComponent <Image>().fillAmount = 1.0f; } else if (i == energy) // filling bar { transform.GetChild(i).GetChild(0).GetComponent <Image>().fillAmount = gauge.AsFloat() - (float)energy.AsFloat(); //Debug.Log(gauge - (float)energy); } else { transform.GetChild(i).GetChild(0).GetComponent <Image>().fillAmount = 0.0f; } i++; } }
private void FillAttractTimeData(int i_SlotIndex, tnStandardMatchController i_MatchController) { if (i_MatchController == null) { return; } int charcatersCount = i_MatchController.charactersCount; // Compute total shots count. FP totalAttractTime = FP.Zero; for (int characterIndex = 0; characterIndex < charcatersCount; ++characterIndex) { tnStandardMatchCharacterResults characterResults = (tnStandardMatchCharacterResults)i_MatchController.GetCharacterResultsByIndex(characterIndex); if (characterResults != null) { totalAttractTime += characterResults.attractTime; } } // Get best character for this stat. int selectedCharacterIndex = -1; int selectedCharacterId = Hash.s_NULL; FP maxAttractTime = FP.MinValue; for (int characterIndex = 0; characterIndex < charcatersCount; ++characterIndex) { tnStandardMatchCharacterResults characterResults = (tnStandardMatchCharacterResults)i_MatchController.GetCharacterResultsByIndex(characterIndex); if (characterResults != null) { FP characterAttractTime = characterResults.attractTime; if (characterAttractTime > maxAttractTime) { selectedCharacterIndex = characterIndex; selectedCharacterId = characterResults.id; maxAttractTime = characterAttractTime; } } } if (selectedCharacterIndex < 0) { return; } // Get team color. Color teamColor = Color.white; GameObject characterGo = i_MatchController.GetCharacterByIndex(selectedCharacterIndex); if (characterGo != null) { tnCharacterInfo characterInfo = characterGo.GetComponent <tnCharacterInfo>(); if (characterInfo != null) { teamColor = characterInfo.teamColor; } } // Fill data. string playerName = ""; Sprite playerPortrait = null; { tnCharacterData characterData = tnGameData.GetCharacterDataMain(selectedCharacterId); if (characterData != null) { playerName = characterData.displayName; playerPortrait = characterData.uiIconFacingRight; } } string statValue = maxAttractTime.ToString(2); statValue += " s"; float partecipationPercentage = 0f; if (totalAttractTime > 0f) { partecipationPercentage = maxAttractTime.AsFloat() / totalAttractTime.AsFloat(); partecipationPercentage *= 100f; partecipationPercentage = Mathf.Clamp(partecipationPercentage, 0f, 100f); } string partecipationValue = partecipationPercentage.ToString("0.00"); partecipationValue += "%"; FillData(i_SlotIndex, playerName, playerPortrait, teamColor, s_StatName_AttractTime, statValue, s_PartecipationLabel, partecipationValue); }
public override void OnSyncedUpdate() { DealSycncedEndInfo(); FPVector changeForwardVec3 = FrameSyncInput.GetFPVector((byte)E_InputId.E_DRAGGING_CAMERA); bool IdelFlag = FrameSyncInput.GetBool((byte)E_InputId.E_IDEL); FP move_x = FrameSyncInput.GetFP((byte)E_InputId.E_MOVE_X); FP move_y = FrameSyncInput.GetFP((byte)E_InputId.E_MOVE_Y); bool mouseDraggingFlag = FrameSyncInput.GetBool((byte)E_InputId.E_MOUSE_DRAGGING); bool mouseDragEndFlag = FrameSyncInput.GetBool((byte)E_InputId.E_MOUSE_DRAGEND); if (mouseDraggingFlag || mouseDragEndFlag) { bool leftDirection = FrameSyncInput.GetBool((byte)E_InputId.E_MOUSE_DRAG_LEFT); bool rightDirection = FrameSyncInput.GetBool((byte)E_InputId.E_MOUSE_DRAG_RIGHT); ComMoveFollowObj.SetMouseDragDirection(leftDirection, rightDirection); if (mouseDraggingFlag) { ComMoveFollowObj.SetMouseDragFlag(true); } else { ComMoveFollowObj.SetMouseDragFlag(false); } } if (changeForwardVec3 != FPVector.zero) { Debug.Log("changeForwardVec3:::::::::::" + changeForwardVec3.ToVector() + ",,,E_InputId.E_IDEL::" + IdelFlag); Debug.Log("FP_FP_FP_::move_x_xxxxxxxx::" + move_x.AsFloat() + ",move_y::" + move_y.AsFloat()); } //移动处理 if (IdelFlag) { Idle(); ComMoveFollowObj.SetJoyStickMoveFlag(false); //Debug.Log("idelidelidelidelidelidelidelidelidelidelidelidelidelidelidel"); } else { if (move_x != 0 || move_y != 0) { if (changeForwardVec3 != FPVector.zero) { Debug.Log("changeForwardVec3::" + changeForwardVec3.ToVector() + "camera::" + ComMoveFollowObj.transform.forward + ",touch:::" + ComMoveFollowObj._bTouchMouse); ChangeAvatarForward(changeForwardVec3); // Debug.Log("changeForwardVec3:::::::::::" + changeForwardVec3.ToVector() +"camera::"+ ComMoveFollowObj.transform.forward); } SycnMove(move_x, move_y); } } //射击处理 bool shootDownFlag = FrameSyncInput.GetBool((byte)E_InputId.E_SHOOT_DOWN); if (shootDownFlag) { Shoot(changeForwardVec3); } bool shootUpFlag = FrameSyncInput.GetBool((byte)E_InputId.E_SHOOT_UP); if (shootUpFlag) { Shoot(FPVector.zero, false); } //跳跃处理 bool jumpFlag = FrameSyncInput.GetBool((byte)E_InputId.E_JUMP); if (jumpFlag) { Jump(); } //模型改变处理 int modelRender = FrameSyncInput.GetInt((byte)E_InputId.E_CHANGE_MODEL); if (modelRender > 0) { if (modelRender == (int)KeyCode.P) //人形 { ChangeAvatarForm(Avatar.E_AvatarForm.PERSON_STATE); } else if (modelRender == (int)KeyCode.F) //乌贼 { ChangeAvatarForm(Avatar.E_AvatarForm.INKFISH_STATE); } else if (modelRender == (int)KeyCode.D) //下潜模式 { ChangeAvatarForm(Avatar.E_AvatarForm.INKFISHDIVE_STATE); } } //更换装备 int replaceWeaponFlag = FrameSyncInput.GetInt((byte)E_InputId.E_REPLACE_WEAPON); if (replaceWeaponFlag > 0) { ChangeWeapon(replaceWeaponFlag); } //扔手雷 bool throwGrenadeFlag = FrameSyncInput.GetBool((byte)E_InputId.E_THROW_GRENADE); if (throwGrenadeFlag) { ThrowGrenade(); } }
public override string ToString() { return(string.Format("({0:f1}, {1:f1})", x.AsFloat(), y.AsFloat())); }
IEnumerator SpawnUnit() { if (randomSeed == 0) { randomSeed = (int)System.DateTime.Now.Ticks; } UnityEngine.Random.InitState(randomSeed); TSRandom random = TSRandom.instance; while (true && countLimit > 0) { for (int i = 0; i < 10 && countLimit > 0; i++) { TestAgent tagent = GameObject.Instantiate <TestAgent>(agent); PathFindingAgentBehaviour unit = s_AstarAgent.New(); if (unit != null) { tagent._testPathFinding = this; tagent.unit = unit; int camp = countLimit % campCount; //start pos Vector3 vPos = startObj[camp].transform.position + new Vector3(UnityEngine.Random.Range(0, 2.0f), 0, UnityEngine.Random.Range(0, 2.0f)); TSVector pos = TSVector.zero; pos.Set(CustomMath.FloatToFP(vPos.x), CustomMath.FloatToFP(vPos.y), CustomMath.FloatToFP(vPos.z)); TSVector sPos = pos; tagent.gameObject.transform.position = CustomMath.TsVecToVector3(sPos); // Debug.Log(pos); //target pos Vector3 tpos = destObj[camp].transform.position; // new Vector3(48.0f, 0.0f, 46.8f); pos.Set(CustomMath.FloatToFP(tpos.x), CustomMath.FloatToFP(tpos.y), CustomMath.FloatToFP(tpos.z)); //get center int idx = _map.GetGridNodeId(pos); pos = _map.GetWorldPosition(idx); TSVector targetPos = pos; FP attackRange = _atkRanges[countLimit % _atkRanges.Length]; FP range = TSMath.Max(attackRange + GridMap.GetNodeSize() * CustomMath.FPHalf, FP.One * 3 * GridMap.GetNodeSize()); AgentBaseData data = new AgentBaseData(); data.id = countLimit; data.mapId = 0; data.playerId = camp; data.eAgentType = EAgentType.none; //data.id%5== 0? EAgentType.ingoreObstacle: #if !USING_FLOW_FIELD data.defaultTargetPos = TSVector.zero; //astar #else data.defaultTargetPos = targetPos; #endif data.loopCallback = tagent.Loop; data.boidsType = (byte)EBoidsActiveType.all; data.maxSpeed = FP.One * maxSpeed; data.position = sPos; data.collidesWithLayer = 1; data.viewRadius = FP.One * 6 * GridMap.GetNodeSize(); data.neighbourRadius = range; data.random = random; data.colliderRadius = 0.5f; //test data.pfm = _pfm; data.groupId = (byte)(data.eAgentType == EAgentType.ingoreObstacle ? 1 : 0); data.targetFailCallback = tagent.FailFindPathCallback; unit.enabled = false; unit.Init(data); EAgentType agentType = EAgentType.astar; #if USING_FLOW_FIELD agentType = EAgentType.flowFiled; #endif unit.ChangeAgentType(data.eAgentType == EAgentType.ingoreObstacle? data.eAgentType : agentType); unit.agent.TargetPos = targetPos; // unit.OnEnable();//????????? tagent.attackRange = attackRange; // FP.One * UnityEngine.Random.Range(0.8f, 5); tagent._attackRange = attackRange.AsFloat(); // unit.AgentType = EAgentType.flowFiled; #if !USING_FLOW_FIELD unit.ChangeAgentType(EAgentType.astar); //astar #endif if (groupCount > 0) { AgentGroupManager.instance.UpdateGroup(unit, countLimit % groupCount); } tagent.transform.GetChild(0).GetComponent <SpriteRenderer>().color = _campColor[camp]; // unit.agent.StartPos = unit.position; // unit._agent.TargetPos = (TSVector)destination; tagent.gameObject.name = "unit" + countLimit; tagent.transform.SetParent(Units); // unit.agent.TargetPos = targetPos; // unit.agent._activeBoids = (byte)EBoidsActiveType.all; _listAgents.Add(tagent); // PathFindingManager.Instance.AddAgent(this); //_pm.FindFastPath(unit._agent, unit._agent.StartPos, unit._agent.TargetPos);//, unit._agent._vecPath // break; if (unit.group != null && (unit.group.leader as PathFindingAgentBehaviour) == unit && unit.AgentType == EAgentType.astar) { _pm.FindQuickPath(unit.agent, 10, unit.agent.StartPos, unit.agent.TargetPos, unit.map, false); } //if(unit.group!=null) //{ // unit.agent._activeBoids = (byte)EBoidsActiveType.alignment& (byte)EBoidsActiveType.cohesion & (byte)EBoidsActiveType.terrainSeperation //} countLimit--; if (countLimit % 5 == 0) { yield return(_wait); } } } } yield return(null); }
public void Update() { if (_destVector != FPVector.zero) { float disData = Vector3.Distance(transform.position, _ufpTransform.position.ToVector()); FP fixDistan = MoveSpeed * Time.deltaTime; if (disData <= fixDistan) { UpdatePosition(_ufpTransform.position); UpdateRotation(_ufpTransform.rotation); // Debug.Log("======>arrived_____arrived_____arrived_____arrived_____arrived:::" + _ufpTransform.position); } else { transform.position += (_childObj.forward * fixDistan).ToVector(); // Debug.Log("======>_ufpTransform.position_OnLine:::"+ _ufpTransform.position); } } if (_destRoundData != 0) { FP angleData = _destRoundData - _roundData; FP _tempData = RotationSpeed * MoveSpeed * Time.deltaTime; //Debug.Log("======>angleData:::" + angleData.AsFloat() // + ",_tempData::" + _tempData.AsFloat() + ",_destRoundData::"+ _destRoundData); if (FPMath.Abs(angleData) <= FPMath.Abs(_tempData)) { UpdatePosition(_ufpTransform.position); UpdateRotation(_ufpTransform.rotation); _roundData = _destRoundData; //Debug.Log("======>arrived_____round_____arrived_____round_____arrived:::" + _ufpTransform.position // + ",childObj.forward::" + _childObj.forward + ",childObj.forward.toVector3::" + _childObj.forward.ToVector()); } else { transform.RotateAround(_center.ToVector(), transform.up, _tempData.AsFloat()); _roundData += _tempData; Debug.Log("======>_ufpTransform.round_round_OnLine:::" + _ufpTransform.position + ",_ufpTransform.position.toVector3::" + _ufpTransform.position // + ", ufpTransform.rotation:::" + _ufpTransform.rotation // + ",_ufpTransform.rotation.toVector()::" + _ufpTransform.rotation.ToQuaternion() + ",childObj.forward::" + _childObj.forward + ",childObj.forward.toVector3::" + _childObj.forward.ToVector()); } } RaycastHit hitInfo; bool bExitBarrier = (RayBarrierInfo(_childObj.forward.ToVector(), out hitInfo)); RaycastHit _groundHit; RaycastHit _hitInfo; if (bJumpFlag) //处理跳跃 { if (!bJumpArtFlag) //表示第一次开始跳跃 { if (!RayGroundInfo(out _groundHit, true)) { bJumpArtFlag = true; } } else { if (RayGroundInfo(out _groundHit, true)) { bJumpFlag = false; bJumpArtFlag = false; return; } } _jumpSpeed = _jumpSpeed - g * FrameSyncManager.DeltaTime; //垂直上的初速度随时间的改变 _ufpTransform.Translate(FPVector.up * _jumpSpeed * FrameSyncManager.DeltaTime, Space.World); //垂直上的运动 UpdateRotation(_ufpTransform.rotation); UpdatePosition(_ufpTransform.position); //transform.Translate(Vector3.up * _jumpSpeed * FrameSyncManager.DeltaTime, Space.World);//垂直上的运动 //if (_jumpSpeed < 0 && RayGroundInfo(out _groundHit, true)) //增加防止坠落的操作 //{ // transform/*.parent*/.position = new Vector3(transform/*.parent*/.position.x, _groundHit.point.y, transform/*.parent*/.position.z); // bJumpFlag = false; // bJumpArtFlag = false; // return; //} if (bMoveFlag) { _ufpTransform.Translate(_childObj.forward * MoveSpeed * FrameSyncManager.DeltaTime, Space.World);//水平上的运动 UpdateRotation(_ufpTransform.rotation); UpdatePosition(_ufpTransform.position); // transform.Translate(_childObj.forward.ToVector() * (MoveSpeed/** _frameEntityObj.SpeedRate*/).AsFloat() * Time.fixedDeltaTime, Space.World);//水平上的运动 } return; } else if (!RayGroundInfo(out _groundHit) && !RayBarrierInfo(_childObj.forward.ToVector(), out _hitInfo) && _bHasSpeed)//空中调整角度 { //Debug.Log("_childObj.forward.ToVector():::" + _childObj.forward.ToVector() // + ",,,,data__ufpTransform.up::" + _ufpTransform.up.ToVector() + ",,,,,fff::"+ _ufpTransform.forward.ToVector()); _verCurSpeed = _verCurSpeed - g * FrameSyncManager.DeltaTime; //垂直上的初速度随时间的改变 _ufpTransform.Translate(_childObj.forward * MoveSpeed * FrameSyncManager.DeltaTime, Space.World); //水平上的运动 _ufpTransform.Translate(FPVector.up * _verCurSpeed * FrameSyncManager.DeltaTime, Space.World); //垂直上的运动 UpdateRotation(_ufpTransform.rotation); UpdatePosition(_ufpTransform.position); // transform.Translate(_childObj.forward.ToVector() * (MoveSpeed/** _frameEntityObj.SpeedRate*/).AsFloat() * Time.fixedDeltaTime, Space.World);//水平上的运动 // transform.Translate(Vector3.up * _verCurSpeed.AsFloat() * Time.fixedDeltaTime, Space.World);//垂直上的运动 FP angleForward = FPVector.Angle(_ufpTransform.up, FPVector.up); if (angleForward == 0) { return; } FPVector normal = FPVector.Cross(_ufpTransform.up, FPVector.up); // int DirctData = FPMath.Sign(FPVector.Dot(normal, _ufpTransform.up)); float DirctData = Mathf.Sign(Vector3.Dot(normal.ToVector(), _ufpTransform.up.ToVector())); //Debug.Log(" angleForward::" + angleForward.AsFloat() + ",DirctData::"+ DirctData + ",_ufpTransform.up::"+ _ufpTransform.up + " ,"+ _ufpTransform.up.ToVector() + ",FPVector.up::"+ FPVector.up.ToVector() // + ",normal::" + normal + "," + normal.ToVector() + ", FPVector.Dot(normal, _ufpTransform.up)::" + FPVector.Dot(normal, _ufpTransform.up).AsFloat()); //if (DirctData == 0) DirctData = 1; angleForward = angleForward * DirctData; // Debug.Log(" FPMath.Sign(FPVector.Dot(normal, _ufpTransform.up)::" + FPVector.Dot(new FPVector(0,0,1), new FPVector(1, 0, 0))); FPVector forwardVec3 = FPQuaternion.AngleAxis(angleForward, normal) * _ufpTransform.up; FPVector forwardForward = FPQuaternion.AngleAxis(angleForward, normal) * _ufpTransform.forward; FPQuaternion qur = FPQuaternion.LookRotation(forwardForward, forwardVec3); //Debug.Log("forwardForward:::" + forwardForward.ToVector() + ",,forwardVec3::" + forwardVec3.ToVector() // + ",angleForward::"+ angleForward.AsFloat() // + ",_ufpTransform.up::" + _ufpTransform.up.ToVector() + ", _ufpTransform.forward::" + _ufpTransform.forward.ToVector() // + ",normal::" + normal.ToVector()); UpdateRotation(FPQuaternion.Slerp(_ufpTransform.rotation, qur, 0.1f)); //_ufpTransform.SetRotation(); //transform.rotation = FPQuaternion.Slerp(_ufpTransform.rotation, qur, 0.1f); //将玩家处于空中的状态事件发射出去 TriggerEvent(DefineEventId.PlayerInAirEvent); } else { FP angle = FPVector.Angle(FPVector.up, _ufpTransform.ChangeVec3ToTSVec(_groundHit.normal)); if (angle > SlopeAngle) { if (!bMoveFlag) { _ufpTransform.Translate(-1 * (FPVector.up) * g * FrameSyncManager.DeltaTime, Space.World); UpdateRotation(_ufpTransform.rotation); UpdatePosition(_ufpTransform.position); } } else { // transform.position = new Vector3(transform/*.parent*/.position.x, _groundHit.point.y, transform/*.parent*/.position.z); UpdateRotation(_ufpTransform.rotation); UpdatePosition(new FPVector(_ufpTransform.position.x, (FP)(_groundHit.point.y), _ufpTransform.position.z)); } } }
public void Move() { if (bJumpFlag) { return; } FPVector groundNormal = FPVector.zero; RaycastHit hitInfo; if (!RayGroundInfo(out hitInfo)) { if (!bJumpFlag) { bMoveFlag = false; } return; //如果离地,直接返回 } else { groundNormal = new FPVector(hitInfo.normal.x, hitInfo.normal.y, hitInfo.normal.z); } if (!bMoveFlag) { bMoveFlag = true; } FPVector forward = _ufpTransform.forward; FPVector fpNormal = _ufpTransform.ChangeVec3ToTSVec(hitInfo.normal); RaycastHit barrierInfo; bool bExitBarrier = (RayBarrierInfo(_childObj.forward.ToVector(), out barrierInfo)); //表示前方检测到物体,暂时把检测到的物体都称之为障碍物 bool bSlope = false; //默认不是为斜坡标志 if (bExitBarrier) { hitInfo = barrierInfo; bSlope = (Vector3.Angle(Vector3.up, hitInfo.normal) > SlopeAngle) ? true : false; if (ForWardMode == E_ForWardMode.FM_ON_FORWARD) { forward = _ufpTransform.forward; } else { FP angle = 0; fpNormal = _ufpTransform.ChangeVec3ToTSVec(hitInfo.normal); if (groundNormal != FPVector.zero) { angle = FPVector.Angle(groundNormal, fpNormal); angle = -angle; } if (_childObj) { //Vector3 newCrossVec3 = Vector3.Cross(hitInfo.normal, transform.up); //forward = Quaternion.AngleAxis(angle, newCrossVec3) * (transform.forward); FPVector newCrossVec3 = FPVector.Cross(fpNormal, _ufpTransform.up); forward = FPQuaternion.AngleAxis(angle, newCrossVec3) * _ufpTransform.forward; } } } else { bSlope = ((FPVector.Angle(FPVector.up, fpNormal)) > SlopeAngle) ? true : false; } if (SurfaceMode == E_SurfaceMode.SM_ON_GROUND) //非跨面模式:供人型和乌贼模式下使用 { if (bExitBarrier && bSlope) { return; } } else if (SurfaceMode == E_SurfaceMode.SM_ON_SURFACE) //跨面模式:供乌贼下潜模式下使用 { if (bSlope) { MotionMode = E_MotionMode.MM_ON_LINE; bool flag = CanMoveByInk(ref hitInfo); //上墙后到边角的处理 ==>TODO flag = true; if (!flag) { FPVector position = _ufpTransform.ChangeVec3ToTSVec(hitInfo.point) + (_childObj.forward * MoveSpeed * 0.1f); FPVector currentPosition = _ufpTransform.position; //_ufpTransform.position = position; //transform.position = position.ToVector(); UpdatePosition(_ufpTransform.position); RayGroundInfo(out barrierInfo); flag = CanMoveByInk(ref barrierInfo); // transform.position = currentPosition.ToVector(); UpdatePosition(_ufpTransform.position); if (bExitBarrier || !flag) { return; } } } } if (hitInfo.transform != null && ForWardMode == E_ForWardMode.FM_ON_NORMAL) { FitSurfaceParent(forward, _ufpTransform.ChangeVec3ToTSVec(hitInfo.point), fpNormal); //调整贴面 } else if (hitInfo.transform != null && ForWardMode == E_ForWardMode.FM_ON_FORWARD) { if (transform.up != Vector3.up) { FP quaternionSpeed = 0.09f; FPQuaternion data = new FPQuaternion(0, _ufpTransform.rotation.y, 0, _ufpTransform.rotation.w); _ufpTransform.SetRotation(FPQuaternion.Slerp(_ufpTransform.rotation, data, quaternionSpeed)); } } if (MotionMode == E_MotionMode.MM_ON_ROUND) //画圆模式:按照圆的弧度来行走 { _destRoundData = RotationSpeed * MoveSpeed * FrameSyncManager.DeltaTime; _ufpTransform.RotateAround(_center, _ufpTransform.up, _destRoundData); //_destUfpTransform.position = _ufpTransform.position; //_destUfpTransform.rotation = _ufpTransform.rotation; //_destUfpTransform.RotateAround(_center, _ufpTransform.up, _destRoundData); Debug.Log("----------------->_characterMotion:::" + _destRoundData + ",_destRoundData::" + _destRoundData.AsFloat() + ",childObj.forward::" + _childObj.forward + ",childObj.forward.toVector3::" + _childObj.forward.ToVector()); } else if (MotionMode == E_MotionMode.MM_ON_LINE)//直走模式:按照其forward的方向行走 { _destVector = _childObj.forward * MoveSpeed * FrameSyncManager.DeltaTime; _ufpTransform.Translate(_destVector, Space.World); //_destUfpTransform.position = _ufpTransform.position; //_destUfpTransform.rotation = _ufpTransform.rotation; //_destUfpTransform.Translate(_destVector, Space.World); } // if (Physics.Raycast(RayPointObj.position + (_destUfpTransform.position - _ufpTransform.position).ToVector(), -transform.up, GroundRayLength)) if (Physics.Raycast(RayPointObj.position + (_ufpTransform.position.ToVector() - transform.position), -transform.up, GroundRayLength)) { _bHasSpeed = false; } else { _bHasSpeed = true; RenderEndUpdatePosition(); } _verCurSpeed = MoveSpeed / 2; }
///// <summary> ///// Gets the center of mass of an array of Rigidbodies. ///// </summary> //public static Vector3 GetCenterOfMass(Rigidbody[] rigidbodies) { // Vector3 CoM = Vector3.zero; // float c = 0f; // for (int i = 0; i < rigidbodies.Length; i++) { // if (rigidbodies[i].gameObject.activeInHierarchy) { // CoM += rigidbodies[i].worldCenterOfMass * rigidbodies[i].mass; // c += rigidbodies[i].mass; // } // } // return CoM / c; //} ///// <summary> ///// Gets the velocity of the center of mass of an array of Rigidbodies. ///// </summary> //public static Vector3 GetCenterOfMassVelocity(Rigidbody[] rigidbodies) { // Vector3 CoM = Vector3.zero; // float c = 0f; // for (int i = 0; i < rigidbodies.Length; i++) { // if (rigidbodies[i].gameObject.activeInHierarchy) { // CoM += rigidbodies[i].velocity * rigidbodies[i].mass; // c += rigidbodies[i].mass; // } // } // return CoM / c; //} ///// <summary> ///// Divides an angular acceleration by an inertia tensor. ///// </summary> //public static void DivByInertia(ref Vector3 v, Quaternion rotation, Vector3 inertiaTensor) { // v = rotation * Div(Quaternion.Inverse(rotation) * v, inertiaTensor); //} ///// <summary> ///// Scales an angular acceleration by an inertia tensor ///// </summary> //public static void ScaleByInertia(ref Vector3 v, Quaternion rotation, Vector3 inertiaTensor) { // v = rotation * Vector3.Scale(Quaternion.Inverse(rotation) * v, inertiaTensor); //} ///// <summary> ///// Returns the angular acceleration from one vector to another. ///// </summary> //public static Vector3 GetFromToAcceleration(Vector3 fromV, Vector3 toV) { // Quaternion fromTo = Quaternion.FromToRotation(fromV, toV); // float requiredAccelerationDeg = 0f; // Vector3 axis = Vector3.zero; // fromTo.ToAngleAxis(out requiredAccelerationDeg, out axis); // Vector3 requiredAcceleration = requiredAccelerationDeg * axis * Mathf.Deg2Rad; // return requiredAcceleration / Time.fixedDeltaTime; //} ///// <summary> ///// Returns the angular acceleration from the current rigidbody rotation to Quaternion.identity. ///// Does not guarantee full accuracy with rotations around multiple axes). ///// </summary> //public static Vector3 GetAngularAcceleration(Quaternion fromR, Quaternion toR) { // Vector3 axis = Vector3.Cross(fromR * Vector3.forward, toR * Vector3.forward); // Vector3 axis2 = Vector3.Cross(fromR * Vector3.up, toR * Vector3.up); // float angle = Quaternion.Angle(fromR, toR); // Vector3 acc = Vector3.Normalize(axis + axis2) * angle * Mathf.Deg2Rad; // return acc / Time.fixedDeltaTime; //} ///// <summary> ///// Returns the linear acceleration from one point to another. ///// </summary> //public static Vector3 GetLinearAcceleration(Vector3 fromPoint, Vector3 toPoint) { // return (toPoint - fromPoint) / Time.fixedDeltaTime; //} // /// <summary> // /// The rotation expressed by the joint's axis and secondary axis // /// </summary> // public static Quaternion ToJointSpace(ConfigurableJoint joint) { // Vector3 forward = Vector3.Cross (joint.axis, joint.secondaryAxis); // Vector3 up = Vector3.Cross (forward, joint.axis); // return Quaternion.LookRotation (forward, up); //} ///// <summary> ///// Calculates the inertia tensor for a cuboid. ///// </summary> //public static Vector3 CalculateInertiaTensorCuboid(Vector3 size, float mass) { // float x2 = Mathf.Pow(size.x, 2); // float y2 = Mathf.Pow(size.y, 2); // float z2 = Mathf.Pow(size.z, 2); // float mlp = 1f/12f * mass; // return new Vector3( // mlp * (y2 + z2), // mlp * (x2 + z2), // mlp * (x2 + y2)); //} ///// <summary> ///// Divide all the values in v by the respective values in v2. ///// </summary> //public static Vector3 Div(Vector3 v, Vector3 v2) { // return new Vector3(v.x / v2.x, v.y / v2.y, v.z / v2.z); //} /// <summary> /// align to vector /// </summary> /// <param name="r"></param> /// <param name="alignmentVector"></param> /// <param name="targetVector"></param> /// <param name="stability"></param> /// <param name="speed"></param> public static void AlignToVector(TSRigidBody r, TSVector alignmentVector, TSVector targetVector, FP stability, FP speed, bool debug = false) { if (r == null) { return; } // 围绕着某个轴进行了一个角度的旋转 angleAxis(x,y). x = 角度 y = 轴 // x 角度值为 当前转向速度 * Stability 是一个 radian 值, * 57.29 转为角度值, 除以 speed 后 才是真正的角度值 // part.angularVelocity 是当前旋转速度, 把旋转速度变为轴? Quaternion angleAxis = Quaternion.AngleAxis(r.angularVelocity.magnitude.AsFloat() * 57.29578f * stability.AsFloat() / speed.AsFloat(), r.angularVelocity.ToVector()); // Vector3.Cross 是叉乘, alignmentVector 是当前我们把part 某个轴要对准target Vector 的向量, angleAxis 四元素乘以AlignmentVector 是把alignmentVector 方向调整为四元素方向 // targetVector 是我们想要对准的角度 // 叉乘出来的 a 就是 两个向量的九十度角的向量用来作为我们的旋转轴 Vector3 a = Vector3.Cross(angleAxis * alignmentVector.ToVector(), targetVector.ToVector() * 10f); if (!float.IsNaN(a.x) && !float.IsNaN(a.y) && !float.IsNaN(a.z)) { //Debug.Log(a.ToTSVector()); r.AddTorque(a.ToTSVector() * speed * speed); Debug.DrawRay(r.position.ToVector(), alignmentVector.ToVector() * 0.3f, Color.red, 0f, false); Debug.DrawRay(r.position.ToVector(), targetVector.ToVector() * 0.3f, Color.green, 0f, false); } }
public void Update() { if (Target == null) { return; } // ChangeDirectionByDragging(); //if (_dragging && _directorToLeft && !_bTouchMouse) //左遥感 //{ // _rockerControl = ComMoveController.RockerControl.LeftControl; // _ufpTransform.RotateAround(_playerTransformObj.position, FPVector.up, FrameSyncManager.DeltaTime * RockerSpeed); // _ufpTransform.UpdateAllData(); // Debug.Log("_ufpTransform:::" + _ufpTransform.forward.ToVector()); // //transform.RotateAround(_childObj.position, Vector3.up, Time.deltaTime * RockerSpeed); // return; //} //else if (_dragging && _directorToRight && !_bTouchMouse) //右遥感 //{ // _rockerControl = ComMoveController.RockerControl.RightControl; // _ufpTransform.RotateAround(_playerTransformObj.position, FPVector.up, FrameSyncManager.DeltaTime * -RockerSpeed); // _ufpTransform.UpdateAllData(); // // transform.RotateAround(_childObj.position, Vector3.up, Time.deltaTime * -RockerSpeed); // return; //} //Debug.Log("_currentAngle:::" + _currentAngle + ",_childObj.eulerAngles.y::" + _childObj.eulerAngles.y // + ",transform.rotation::"+ transform.rotation); RaycastHit GroundHitInfo; RaycastHit BarrieHitrInfo; bool bGroundInfoFlag = _motionObj.RayGroundInfo(out GroundHitInfo); bool bBarrierInfoFlag = _motionObj.RayBarrierInfo(_childTransformObj.forward.ToVector(), out BarrieHitrInfo); FPVector fpGroundInfoNormal = FPVector.zero; if (bGroundInfoFlag) { fpGroundInfoNormal = GroundHitInfo.normal.ToFPVector(); } if (!_dragging && !_bTouchMouse && _bBeforeDragging) { _rockerControl = ComMoveController.RockerControl.None; return; } else if (!_dragging) { if (_bBeforeDragging) //在这里设置值,为了保证childObj的forward朝向跟摄像机的一致,防止先后差值 { _bBeforeDragging = false; return; } if (!bGroundInfoFlag) //在空中 { _rockerControl = ComMoveController.RockerControl.AirControl; if (_motionObj.bJumpFlag && false) //这是处理不跟随着对象跳跃的逻辑部分 { Distance = FPMath.Clamp(Distance - (Input.GetAxis("Mouse ScrollWheel") * MouseScrollWheelSensitivity), DistanceMin, DistanceMax); FPVector DataVec3 = new FPVector(_playerTransformObj.position.x, transform.position.y - Height, _playerTransformObj.position.z); _ufpTransform.position = (_ufpTransform.rotation * new FPVector(.0f, Height, -Distance)) + DataVec3; _ufpTransform.UpdateAllData(); return; } } else if (!bBarrierInfoFlag || !_motionObj.JudgetGroundSlope(BarrieHitrInfo.normal)) //有地面接触但前方没有障碍物 { if (!_motionObj.JudgetGroundSlope(GroundHitInfo.normal)) { _rockerControl = ComMoveController.RockerControl.OtherControl; _climbOtherWall = false; } else { if (_climbOtherWall && _beforeWallNormalVec3 != fpGroundInfoNormal) //表示从一面墙跨到另外一面墙 { _beforeGroundNormalVec3 = _beforeWallNormalVec3; _beforeWallNormalVec3 = fpGroundInfoNormal; } _rockerControl = ComMoveController.RockerControl.ClimbControl; } } else //有地面接触且前方有障碍物 { _rockerControl = ComMoveController.RockerControl.None; _beforeControl = ComMoveController.RockerControl.OtherControl; if (!_motionObj.JudgetGroundSlope(GroundHitInfo.normal)) //从地面跨到墙上的情况 { _beforeGroundNormalVec3 = fpGroundInfoNormal; } else //从一面墙跨到另外一面墙的情况 { _climbOtherWall = true; _beforeWallNormalVec3 = fpGroundInfoNormal; //设置这个变量的原因是:有可能检测到障碍物,但是玩家并没有跨越过去 } } } if (_rockerControl == RockerControl.AirControl) { Distance = FPMath.Clamp(Distance - (Input.GetAxis("Mouse ScrollWheel") * MouseScrollWheelSensitivity), DistanceMin, DistanceMax); _ufpTransform.position = (_ufpTransform.rotation * new FPVector(.0f, Height, -Distance)) + _playerTransformObj.position; _ufpTransform.UpdateAllData(); } else if (_rockerControl == RockerControl.OtherControl) { var quaternion = FPQuaternion.AngleAxis((_currentAngle) + _childTransformObj.transform.eulerAngles.y, FPVector.up); Distance = FPMath.Clamp(Distance - (Input.GetAxis("Mouse ScrollWheel") * MouseScrollWheelSensitivity), DistanceMin, DistanceMax); _ufpTransform.SetRotation(quaternion); // transform.rotation = quaternion; FPVector data = (_ufpTransform.rotation * new FPVector(.0f, Height, -Distance)); _ufpTransform.position = (_ufpTransform.rotation * new FPVector(.0f, Height, -Distance)) + _playerTransformObj.position; _ufpTransform.UpdatePosition(); //Debug.Log("data::"+ data.ToVector()+ ", _ufpTransform.position::::" + _ufpTransform.position.ToVector() // + ",transform::" + transform.position + "::_playerTransformObj.position:" + _playerTransformObj.position.ToVector() // + ",,name::" + _playerTransformObj.gameObject.name); _rotation = _ufpTransform.rotation; _ufpTransform.UpdateForward(); } else if (_rockerControl == RockerControl.ClimbControl) { Distance = FPMath.Clamp(Distance - (Input.GetAxis("Mouse ScrollWheel") * MouseScrollWheelSensitivity), DistanceMin, DistanceMax); var quaternion = FPQuaternion.AngleAxis((0) + transform.eulerAngles.y, FPVector.up); _ufpTransform.position = (_ufpTransform.rotation * new FPVector(.0f, Height, -Distance)) + _playerTransformObj.position; _ufpTransform.UpdateAllData(); FPVector climbForward = _ufpTransform.forward; if (_beforeControl == ComMoveController.RockerControl.OtherControl && _beforeGroundNormalVec3 != FPVector.zero) { FP tempAngle = FPVector.Angle(_beforeGroundNormalVec3, fpGroundInfoNormal); FPVector normal = FPVector.Cross(_beforeGroundNormalVec3, fpGroundInfoNormal); //叉乘求出法线向量 // num *= Mathf.Sign(Vector3.Dot(normal, info.transform.up)); //求法线向量与物体上方向向量点乘,结果为1或-1,修正旋转方向 climbForward = FPQuaternion.AngleAxis((90 - tempAngle), normal) * fpGroundInfoNormal; climbForward = -1 * climbForward; _finishWallNormalVec3 = climbForward; _beforeControl = ComMoveController.RockerControl.ClimbControl; } FP forwardAngle = FPVector.Angle(_finishWallNormalVec3, _ufpTransform.forward); if (forwardAngle != 0 && false) //处理摄像机角度偏转 { //1)调整摄像机的旋转角度 float direcFlag = -1; FPVector normalVec3 = FPVector.Cross(_finishWallNormalVec3, _ufpTransform.forward); //叉乘求出法线向量 direcFlag *= FPMath.Sign(Vector3.Dot(normalVec3.ToVector(), _ufpTransform.up.ToVector())); //求法线向量与物体上方向向量点乘,结果为1或-1,修正旋转方向 forwardAngle *= direcFlag; FPVector beforeForward = _ufpTransform.forward; FPVector forward = FPQuaternion.AngleAxis(forwardAngle, _ufpTransform.up) * _ufpTransform.forward; // Debug.Log("_ufpTransform.forward::" + _ufpTransform.forward.ToVector() + ",forward::" + forward.ToVector() // + "forwardAngle:::" + forwardAngle.AsFloat() + ",forward1111::" + forward); float quaternionSpeed = 0.003f; if (!_bTouchMouse) { quaternionSpeed = 0.03f; } if (beforeForward != forward) { Debug.Log("LookRotation(forward):::" + FPQuaternion.LookRotation(forward) + ",_rotation::" + _rotation + ",unity::" + Quaternion.LookRotation(forward.ToVector())); _rotation = FPQuaternion.Slerp(_rotation, FPQuaternion.LookRotation(forward), quaternionSpeed); _ufpTransform.SetRotation(_rotation); } // Debug.Log(",forward::"+ forward.ToVector() + ",_ufpTransform.forward::" + _ufpTransform.forward.ToVector()); //2)调整人物的旋转角度 if (!_climbOtherWall) // 这是从地面爬到墙得处理,如果是从一面墙爬到另外一面墙,镜头不做转换 { Debug.Log("beforeForward:::" + beforeForward.ToVector() + ",_ufpTransform.forward::" + _ufpTransform.forward.ToVector()); _offsetAngle = FPVector.Angle(beforeForward, _ufpTransform.forward) * direcFlag; _avatarObj.ChangeAvaterForward(_offsetAngle); } } } Debug.DrawLine(_ufpTransform.position.ToVector(), _playerTransformObj.transform.position, Color.red); if (_rockerControl == RockerControl.OtherControl || _rockerControl == RockerControl.ClimbControl) { //看是否有障碍物 FPVector directionTarget = (_ufpTransform.position - _ufpTransform.ChangeVec3ToTSVec(_rayPointObj.position)).normalized; FP distance = FPVector.Distance(_ufpTransform.position, _ufpTransform.ChangeVec3ToTSVec(_rayPointObj.position)); if (distance > Distance) { _ufpTransform.Translate(directionTarget * (distance - Distance)); _ufpTransform.UpdateRotationAndPosition(); } // Debug.DrawRay(_rayPointObj.position, directionTarget * Distance, Color.black); int layerMask = LayerMask.GetMask(Layers.Render); RaycastHit info; if (Physics.Raycast(_rayPointObj.position, directionTarget.ToVector(), out info, Distance.AsFloat(), layerMask)) //如果 { // Debug.Log("info.name::" + info.transform.name); if (info.transform.name != transform.name /*&& info.transform.tag != Tags.Ground*/) { _ufpTransform.SetPosition(_ufpTransform.ChangeVec3ToTSVec(info.point)); //transform.position = info.point; } if (_rockerControl == RockerControl.OtherControl) { _beforeControl = RockerControl.OtherControl; } } } }
internal static TSVector2 computeForces(IAgentBehaviour behaviour, List <CircleObstacleAngleData> circleObstacles, TSVector basicVelocity, bool bUseForwardPos, FP maxTime, FP obstacleMaxTime, out bool isTminStaticAgent, out FP time, ref bool isCollidering, bool bIgnoreObstacle = false)//, { time = -1; TSVector2 F = TSVector2.zero; List <IAgentBehaviour> neighbours = behaviour.neighbours; int icount = neighbours.Count; isTminStaticAgent = false; FP tmin = FP.MaxValue; TSVector forwardPos = TSVector.zero; if (bUseForwardPos) { forwardPos = behaviour.position + basicVelocity.normalized * behaviour.colliderRadius; } //return TSVector2.zero; TSVector pos = (bUseForwardPos ? forwardPos : behaviour.position); // compute the anticipatory force from each neighbor for (int i = 0; i < icount; ++i) { IAgentBehaviour other = neighbours[i]; FP radiusSum = other.colliderRadius;// + behaviour.colliderRadius; if (!bUseForwardPos) { radiusSum = other.colliderRadius + behaviour.colliderRadius; } if (behaviour != other) { maxTime = other.agent == null? obstacleMaxTime:maxTime; F += ComputeForce(behaviour, pos, other.position, radiusSum, other.agent == null, maxTime, basicVelocity, other.velocity, ref isTminStaticAgent, ref tmin, ref time, ref isCollidering); #if UNITY_EDITOR && !MULTI_THREAD if (PathFindingManager.DEBUG && behaviour.agent != null) { if (TSMath.Abs(F.x) > behaviour.baseData.maxForce * 1000 || TSMath.Abs(F.y) > behaviour.baseData.maxForce * 1000) { UnityEngine.Debug.Log("F over flow!"); } } #endif } } // if (!bIgnoreObstacle) { FP dirDst = GridMap.blockDirDst; TSVector testDir = basicVelocity.normalized; TSVector pos2 = behaviour.position + testDir * dirDst; behaviour.pathManager._queryStack.Clear(); TSVector blockedPos = TSVector.zero; bool hasObstacle = behaviour.map.IsBlockedByObstacleBetween2Point(behaviour.position, pos2 , behaviour.pathManager._queryStack, ref blockedPos); if (hasObstacle) { F = F - CustomMath.TSVecToVec2(testDir) * behaviour.baseData.maxForce; time = FP.EN1;//near static obstacle isTminStaticAgent = true; } } // behaviour.pathManager._queryStack.Clear(); // //icount = circleObstacles.Count; //bool temp=false; //for (int i = 0; i < icount; ++i) //{ // CircleObstacle other = circleObstacles[i]; // FP radiusSum = other._radius;// + behaviour.colliderRadius; // if (!bUseForwardPos) // { // radiusSum = other._radius + behaviour.colliderRadius; // } // maxTime = obstacleMaxTime; // F += ComputeForce(behaviour, pos, CustomMath.TSVec2ToVec(other._center), radiusSum, true, maxTime, // basicVelocity, TSVector.zero, ref isTminStaticAgent, ref tmin, ref time,ref temp); //} #if USE_OBSTACLE //forces from static obstacles List <LineObstacle> obstacleNeighbours = behaviour.neighbourObstacles; icount = obstacleNeighbours.Count; TSVector2 position = CustomMath.TSVecToVec2(behaviour.position); TSVector2 velocity = CustomMath.TSVecToVec2(basicVelocity); FP radiusSqr = behaviour.colliderRadius * behaviour.colliderRadius; FP neighbourDstSqr = behaviour.neighbourRadius * behaviour.neighbourRadius; for (int i = 0; i < icount; ++i) { LineObstacle obstacle = obstacleNeighbours[i]; TSVector2 n_w = CustomMath.ClosestPointLineSegment(obstacle._p1, obstacle._p2, position) - position; FP d_w = n_w.LengthSquared(); if (velocity * n_w < 0 || d_w == radiusSqr || d_w > neighbourDstSqr) // Agent is moving away from obstacle, already colliding or obstacle too far away { continue; } FP radius = d_w < radiusSqr?TSMath.Sqrt(d_w) : behaviour.colliderRadius; // correct the radius, if the Agent is already colliding FP a = velocity * velocity; bool discCollision = false, segmentCollision = false; FP t_min = FP.MaxValue; FP c = FP.Zero, b = FP.Zero, discr = FP.Zero; FP b_temp = FP.Zero, discr_temp = FP.Zero, c_temp = FP.Zero, D_temp = FP.Zero; TSVector2 w_temp = TSVector2.zero, w = TSVector2.zero, o1_temp = TSVector2.zero , o2_temp = TSVector2.zero, o_temp = TSVector2.zero, o = TSVector2.zero, w_o = TSVector2.zero; // time-to-collision with disc_1 of the capped rectangle (capsule) w_temp = obstacle._p1 - position; b_temp = w_temp * velocity; c_temp = w_temp * w_temp - (radius * radius); discr_temp = b_temp * b_temp - a * c_temp; if (discr_temp > 0 && (a < -CustomMath.EPSILON || a > CustomMath.EPSILON)) { discr_temp = TSMath.Sqrt(discr_temp); FP t = (b_temp - discr_temp) / a; if (t > 0 && t < C_MaxColliderTime) { t_min = t; b = b_temp; discr = discr_temp; w = w_temp; c = c_temp; discCollision = true; } } // time-to-collision with disc_2 of the capsule w_temp = obstacle._p2 - position; b_temp = w_temp * velocity; c_temp = w_temp * w_temp - (radius * radius); discr_temp = b_temp * b_temp - a * c_temp; if (discr_temp > 0 && (a < -CustomMath.EPSILON || a > CustomMath.EPSILON)) { discr_temp = TSMath.Sqrt(discr_temp); FP t = (b_temp - discr_temp) / a; if (t > 0 && t < t_min) { t_min = t; b = b_temp; discr = discr_temp; w = w_temp; c = c_temp; discCollision = true; } } // time-to-collision with segment_1 of the capsule o1_temp = obstacle._p1 + radius * obstacle._normal; o2_temp = obstacle._p2 + radius * obstacle._normal; o_temp = o2_temp - o1_temp; D_temp = CustomMath.det(velocity, o_temp); if (D_temp != 0) { FP inverseDet = 1 / D_temp; FP t = CustomMath.det(o_temp, position - o1_temp) * inverseDet; FP s = CustomMath.det(velocity, position - o1_temp) * inverseDet; if (t > 0 && s >= 0 && s <= 1 && t < t_min) { t_min = t; o = o_temp; w_o = position - o1_temp; discCollision = false; segmentCollision = true; } } // time-to-collision with segment_2 of the capsule o1_temp = obstacle._p1 - radius * obstacle._normal; o2_temp = obstacle._p2 - radius * obstacle._normal; o_temp = o2_temp - o1_temp; D_temp = CustomMath.det(velocity, o_temp); if (D_temp != 0) { FP inverseDet = 1 / D_temp; FP t = CustomMath.det(o_temp, position - o1_temp) * inverseDet; FP s = CustomMath.det(velocity, position - o1_temp) * inverseDet; if (t > 0 && s >= 0 && s <= 1 && t < t_min) { t_min = t; o = o_temp; w_o = position - o1_temp; discCollision = false; segmentCollision = true; } } bool bMax = false; TSVector2 val = TSVector2.zero; if (discCollision) { if (t_min < FP.EN1 * 2) { bMax = true; // F += (-(velocity - (b * velocity - a * w) / discr)).normalized * behaviour.baseData.maxForce; } else { val = -_k *System.Math.Exp((-t_min / _t0).AsFloat()) * (velocity - (b * velocity - a * w) / discr) / (a * System.Math.Pow(t_min.AsFloat(), _m)) * (_m / t_min + FP.One / _t0); } if (bMax || TSMath.Abs(val.x) > behaviour.baseData.maxForce || TSMath.Abs(val.y) > behaviour.baseData.maxForce) { val = (-(velocity - (b * velocity - a * w) / discr)).normalized * behaviour.baseData.maxForce; } F += val; } else if (segmentCollision) { if (t_min < FP.EN1 * 2) { bMax = true; // F += (CustomMath.det(velocity, o)* new TSVector2(-o.y, o.x)).normalized * behaviour.baseData.maxForce; } else { val = _k * System.Math.Exp((-t_min / _t0).AsFloat()) / (System.Math.Pow(t_min.AsFloat(), _m) * CustomMath.det(velocity, o)) * (_m / t_min + FP.One / _t0) * new TSVector2(-o.y, o.x); } if (bMax || TSMath.Abs(val.x) > behaviour.baseData.maxForce || TSMath.Abs(val.y) > behaviour.baseData.maxForce) { val = (CustomMath.det(velocity, o) * new TSVector2(-o.y, o.x)).normalized * behaviour.baseData.maxForce; } F += val; } #if UNITY_EDITOR if (PathFindingManager.DEBUG && behaviour.agent != null) { if (TSMath.Abs(F.x) > behaviour.baseData.maxForce * 1000 || TSMath.Abs(F.y) > behaviour.baseData.maxForce * 1000) { UnityEngine.Debug.Log("F over flow!"); } } #endif } #endif //if(desiredF.LengthSquared()<F.LengthSquared()) { return(F);// + desiredF; } //else //{ // return desiredF; //} }