private void OnTriggerEnter(Collider other)
{
if (other.gameObject.name == "Player")
{
realRoom.GetComponent <Transform>().position = GeneralVector3.RotateCounterClockwise(GeneralVector3.Vector3NoHeight(player.position), realRoom.GetComponent <Transform>().position, resetAngle);
}
}
//TODO
//private void MovePlayer()
//{
// int num = NearestMovenableWayPoint();
// if (wayPoint.wayPoints[num].turnType == WayPoint.turnType.straight) playerTransform.position = MoveStraight(playerTransform, wayPoint.wayPoints[num].transform);
// else if(wayPoint.wayPoints[num].turnType == WayPoint.turnType.ninetyLeft)
//}
//Update player position
private void MovePlayer(Player player)
{
int moveIndex = NearestMoveableWayPoint();
Ray direction = new Ray(GeneralVector3.Vector3NoHeight(player.transform.position), wayPoint.wayPoints[moveIndex].transform.position - GeneralVector3.Vector3NoHeight(player.transform.position));
//Debug.Log(direction);
player.transform.position = direction.GetPoint(player.velocity * Time.deltaTime) + new Vector3(0, 0.5f, 0);
}
private DataRecord.WayPointsReal[] RotateWayPoints(Vector3 playerPos, DataRecord.WayPointsReal[] rotateWayPoint, float angle)
{
int length = rotateWayPoint.Length;
DataRecord.WayPointsReal[] newWayPoints = new DataRecord.WayPointsReal[length];
for (int i = 0; i < length; ++i)
{
newWayPoints[i].realPosition = GeneralVector3.RotateCounterClockwise(playerPos, rotateWayPoint[i].realPosition, angle);
newWayPoints[i].turnType = rotateWayPoint[i].turnType;
}
return(newWayPoints);
}
private void Awake()
{
count = 0;
orientationSequence = new List <float>();
playerTransform = GameObject.Find("Player").GetComponent <Transform>();
timer = 0;
timerOrientation = 0;
timerVelocity = 0;
carryBit = 0;
timerDataRecord = 0;
presentPosition = lastPosition = GeneralVector3.Vector3NoHeight(playerTransform.position);
presentDirection = lastDirection = playerTransform.rotation.eulerAngles.y;
Corner1 = GameObject.Find("Corner1").GetComponent <Transform>();
Corner2 = GameObject.Find("Corner2").GetComponent <Transform>();
Corner3 = GameObject.Find("Corner3").GetComponent <Transform>();
Corner4 = GameObject.Find("Corner4").GetComponent <Transform>();
Center = GameObject.Find("Center").GetComponent <Transform>();
wayPoint = gameObject.GetComponent <WayPoint>();
action = Redirector.ActionTaken.Zero;
}
//virtual simulate VR Redirection
public void ApplyRedirection(RedirectorType type, Transform player, float deltaPos, float deltaDiv, bool clockwise = true, float gain = 0, float radius = 0)
{
float rotateAngle;
switch (type)
{
case RedirectorType.Absent: break;
case RedirectorType.Curvature:
rotateAngle = deltaPos * 360 / (2 * Mathf.PI * radius);
if (!clockwise)
{
rotateAngle = -rotateAngle;
}
sceneObject.GetComponent <Transform>().RotateAround(GeneralVector3.Vector3NoHeight(player.position), Vector3.up, rotateAngle);
break;
case RedirectorType.Rotation:
rotateAngle = deltaDiv * (1 - gain); //与VR redirection存在差别
sceneObject.GetComponent <Transform>().RotateAround(GeneralVector3.Vector3NoHeight(player.position), Vector3.up, rotateAngle);
break;
}
}
private void CalculateDeltaPar()//Calculate deltaDiv and deltaPos
{
//distance
presentPosition = GeneralVector3.Vector3NoHeight(playerTransform.position);
deltaDistance = Vector3.Distance(presentPosition, lastPosition);
distance += deltaDistance;
lastPosition = presentPosition;
//rotation
presentDirection = playerTransform.rotation.eulerAngles.y;
if (presentDirection - lastDirection > 300)
{
deltaDiv = -(360 - (presentDirection - lastDirection));
}
else if (presentDirection - lastDirection < -300)
{
deltaDiv = 360 + presentDirection - lastDirection;
}
else
{
deltaDiv = presentDirection - lastDirection;
}
lastDirection = presentDirection;
}
//ZeroAction
private Vector3 ZeroAcition(DataRecord.WayPointsReal playerPos, DataRecord.WayPointsReal wayPoint, float velocity = 0)
{
Vector3 endPosition = GeneralVector3.LineDistance(playerPos.realPosition, wayPoint.realPosition, velocity * timeHorizon);
return(endPosition);
}
public MPCResult MPCRedirect(DataRecord.Data data, DataRecord.WayPointsReal[] wayPoints, int depth)
{
MPCResult result = new MPCResult {
cost = 0, action = ActionTaken.Zero
};
if (depth == 0)
{
return(result);
}
else
{
MPCResult tempResult = new MPCResult {
};
MPCResult loopBestResult = new MPCResult {
cost = Mathf.Infinity
};
int matchNum = NearestWayPoint(data.realPosition, wayPoints);
//foreach(var wayPoint in wayPoints)
//{
// Debug.Log(wayPoint.realPosition);
//}
//loop action
for (int i = 0; i < MPCResultNum; ++i)
{
int nearestNumTemp;
float distanceCost;
float parallelCost;
float rotationAngle;
Vector3 endPosition;
Vector3 endDir;
DataRecord.WayPointsReal[] tempWayPoints;
DataRecord.Data tempPlayerData = new DataRecord.Data();
switch ((ActionTaken)i)
{
case ActionTaken.Zero:
//foreach(var wayPoint in wayPoints)
//{
// Debug.Log(wayPoint.realPosition);
//}
endPosition = ZeroAcition(wayPoints[matchNum], wayPoints[matchNum + 1], data.velocity);
//Debug.Log(endPosition);
nearestNumTemp = NearestWayPoint(endPosition, wayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(wayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, wayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.Zero;
tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, wayPoints, depth - 1).cost;
break;
case ActionTaken.PositiveRotation:
if (rotationCost > loopBestResult.cost)
{
break;
}
else if (wayPoints[matchNum].turnType != WayPoint.turnType.ninetyLeft && wayPoints[matchNum].turnType != WayPoint.turnType.ninetyRight)
{
break;
}
else
{
if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyRight)
{
rotationAngle = (rotateGainEnlarge - 1) * 90;
tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle);
endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity);
nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.PositiveRotation;
tempResult.cost = distanceCost + parallelCost + rotationCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost;
}
else if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyLeft)
{
rotationAngle = 360 - (rotateGainEnlarge - 1) * 90;
tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle);
endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity);
nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.PositiveRotation;
tempResult.cost = rotationCost + distanceCost + parallelCost + rotationCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost;
}
}
break;
case ActionTaken.NegativeRotation:
if (rotationCost > loopBestResult.cost)
{
break;
}
else if (wayPoints[matchNum].turnType != WayPoint.turnType.ninetyLeft && wayPoints[matchNum].turnType != WayPoint.turnType.ninetyRight)
{
break;
}
else
{
if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyRight)
{
rotationAngle = 360 - (1 - rotateGainDecrease) * 90;
tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle);
endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity);
nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
Debug.Log(distanceCost);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.NegativeRotation;
tempResult.cost = rotationCost + distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost;
}
else if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyLeft)
{
rotationAngle = (1 - rotateGainDecrease) * 90;
tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle);
endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity);
nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.NegativeRotation;
tempResult.cost = rotationCost + distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost;
}
}
break;
case ActionTaken.PositiveReset:
if (resetCost > loopBestResult.cost)
{
break;
}
else
{
tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, 180);
endPosition = ZeroAcition(tempWayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity);
nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints);
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.PositiveReset;
tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost + resetCost;
break;
}
//case ActionTaken.NegativeReset:
// if (resetCost > loopBestResult.cost) break;
// else break;
case ActionTaken.ClockwiseCurvature:
//Debug.Log("Here"+ loopBestResult.cost);
if (curvatureCost > loopBestResult.cost)
{
break;
}
else
{
tempWayPoints = wayPoints;
endPosition = tempWayPoints[matchNum].realPosition;
int tempMatchNum = matchNum;
nearestNumTemp = matchNum;
float deltaPos = data.velocity * timeHorizon / curvatureCalFrequence;
float deltaAngle = 360 - (deltaPos * 360 / (2 * Mathf.PI * curvatureRadius));
for (int j = 0; j < curvatureCalFrequence; j++)
{
endPosition = GeneralVector3.LineDistance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition, deltaPos);
if (Vector3.Distance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition) < distanceThreshlod)
{
tempMatchNum++;
nearestNumTemp = tempMatchNum;
}
tempWayPoints = RotateWayPoints(endPosition, tempWayPoints, deltaAngle);
}
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
//Debug.Log(distanceCost);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.ClockwiseCurvature;
tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost + curvatureCost;
break;
}
case ActionTaken.CounterClockwiseCurvature:
if (curvatureCost > loopBestResult.cost)
{
break;
}
else
{
tempWayPoints = wayPoints;
endPosition = tempWayPoints[matchNum].realPosition;
int tempMatchNum = matchNum;
nearestNumTemp = matchNum;
float deltaPos = data.velocity * timeHorizon / curvatureCalFrequence;
float deltaAngle = deltaPos * 360 / (2 * Mathf.PI * curvatureRadius);
for (int j = 0; j < curvatureCalFrequence; j++)
{
endPosition = GeneralVector3.LineDistance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition, deltaPos);
if (Vector3.Distance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition) < distanceThreshlod)
{
tempMatchNum++;
nearestNumTemp = tempMatchNum;
}
tempWayPoints = RotateWayPoints(endPosition, tempWayPoints, deltaAngle);
}
distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor);
endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z);
parallelCost = ParallelWallCost(endDir, parallelFactor);
tempPlayerData.realPosition = endPosition;
tempPlayerData.velocity = data.velocity;
tempResult.action = ActionTaken.CounterClockwiseCurvature;
tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost + curvatureCost;
break;
}
default: break;
}
if (tempResult.cost < loopBestResult.cost)
{
loopBestResult = tempResult;
}
}
//Debug.Log(loopBestResult.cost);
result = loopBestResult;
//multply declinefactor
if (depth != timeDepth)
{
result.cost *= declineFactor;
}
//Debug.Log(result.cost);
return(result);
}
}
private void InitPlayerState()
{
lastPos = GeneralVector3.Vector3NoHeight(playerTransform.position);
lastDiv = playerTransform.rotation.eulerAngles.y;
}
// Update is called once per frame
void Update()
{
TimerIncrease();
CalculateDeltaPar();
MovePlayer(player);
//Apply Redirection
switch (action)
{
case Redirector.ActionTaken.Zero: break;
case Redirector.ActionTaken.PositiveRotation:
redirector.ApplyRedirection(Redirector.RedirectorType.Rotation, player.transform, deltaDistance, deltaDiv, gain: redirector.rotateGainEnlarge);
break;
case Redirector.ActionTaken.NegativeRotation:
redirector.ApplyRedirection(Redirector.RedirectorType.Rotation, player.transform, deltaDistance, deltaDiv, gain: redirector.rotateGainDecrease);
break;
case Redirector.ActionTaken.ClockwiseCurvature:
redirector.ApplyRedirection(Redirector.RedirectorType.Curvature, player.transform, deltaDistance, deltaDiv, clockwise: true, radius: redirector.curvatureRadius);
break;
case Redirector.ActionTaken.CounterClockwiseCurvature:
redirector.ApplyRedirection(Redirector.RedirectorType.Curvature, player.transform, deltaDistance, deltaDiv, clockwise: false, radius: redirector.curvatureRadius);
break;
default: break;
}
//velocity data record
if ((timer - timerVelocity * velocityInterval) >= velocityInterval)
{
velocity = distance / velocityInterval;
timerVelocity++;
distance = 0;
}
//exponentially smoothed orientation
if ((timerOrientation - timerOrientation * orientationInterval) >= orientationInterval)
{
//ExponentiallySmoothedSample(true);
}
else
{
//ExponentiallySmoothedSample(false);
}
//data record
if ((timer - timeHorizon * timerDataRecord) >= timeHorizon)
{
data.velocity = velocity;
data.virtualOrientation = player.transform.eulerAngles.y;//没有使用指数平滑
data.virtualPosition = GeneralVector3.Vector3NoHeight(playerTransform.position);
Vector3 originPoint = GeneralVector3.Vector3NoHeight(Corner4.position);
//Debug.Log(originPoint);
Vector3 xAxis = GeneralVector3.Vector3NoHeight(Corner3.position);
//Debug.Log(xAxis);
Vector3 zAxis = GeneralVector3.Vector3NoHeight(Corner1.position);
//Debug.Log(zAxis);
data.realPosition = GeneralVector3.GetRealPoint(originPoint, xAxis, zAxis, data.virtualPosition);
//Debug.Log(data.realPosition);
data.realOrientation = player.transform.eulerAngles.y - Center.rotation.eulerAngles.y;
timerDataRecord++;
WayPointsReal[] wayPointsReals = CalWayPoint2DCoor(wayPoint.wayPoints, playerTransform, wayPoint.wayPoints.Length, data.realOrientation, data.velocity * redirector.timeDepth * timeHorizon * 2, originPoint, xAxis, zAxis);
//Debug.Log(data.realPosition);
//Debug.Log(wayPointsReals[0].realPosition);
action = redirector.MPCRedirect(data, wayPointsReals, redirector.timeDepth).action;
Debug.Log(action);
}
}
private WayPointsReal[] CalWayPoint2DCoor(WayPoint.WayPointSequence[] wayPoints, Transform playerPos, int length, float orientation, float routineDistance, Vector3 origin, Vector3 xAxis, Vector3 zAxis)
{
int nearestnum = NearestMoveableWayPoint();
int largerNum, smallerNum;
if (nearestnum == length - 1)
{
largerNum = 0;
}
else
{
largerNum = nearestnum + 1;
}
if (nearestnum == 0)
{
smallerNum = length - 1;
}
else
{
smallerNum = nearestnum - 1;
}
int assumedNum = Mathf.CeilToInt(routineDistance) + 1;
WayPointsReal[] resultWayPoints = new WayPointsReal[assumedNum];
float anglelarger = Vector3.Angle(GeneralVector3.Vector3NoHeight(playerPos.forward), GeneralVector3.Vector3NoHeight(wayPoints[largerNum].transform.position) - GeneralVector3.Vector3NoHeight(playerPos.position));
float anglesmaller = Vector3.Angle(GeneralVector3.Vector3NoHeight(playerPos.forward), GeneralVector3.Vector3NoHeight(wayPoints[smallerNum].transform.position) - GeneralVector3.Vector3NoHeight(playerPos.position));
if (anglelarger >= anglesmaller)
{
int index = smallerNum;
for (int i = 0; i < assumedNum; ++i)
{
resultWayPoints[i].realPosition = GeneralVector3.GetRealPoint(origin, xAxis, zAxis, GeneralVector3.Vector3NoHeight(wayPoints[index].transform.position));
resultWayPoints[i].turnType = wayPoints[index].turnType;
if (index == 0)
{
index = length - 1;
}
else
{
--index;
}
}
}
else
{
int index = largerNum;
for (int i = 0; i < assumedNum; ++i)
{
resultWayPoints[i].realPosition = GeneralVector3.GetRealPoint(origin, xAxis, zAxis, GeneralVector3.Vector3NoHeight(wayPoints[index].transform.position));
resultWayPoints[i].turnType = wayPoints[index].turnType;
if (index == length - 1)
{
index = 0;
}
else
{
++index;
}
}
}
return(resultWayPoints);
}
