/// <summary>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(FPVector force, FPVector pos)
{
FPVector.Add(ref this.force, ref force, out this.force);
FPVector.Subtract(ref pos, ref this.position, out pos);
FPVector.Cross(ref pos, ref force, out pos);
FPVector.Add(ref pos, ref this.torque, out this.torque);
}
public void GetNormal(out FPVector normal)
{
FPVector sum;
FPVector.Subtract(ref owner.points[indices.I1].position, ref owner.points[indices.I0].position, out sum);
FPVector.Subtract(ref owner.points[indices.I2].position, ref owner.points[indices.I0].position, out normal);
FPVector.Cross(ref sum, ref normal, out normal);
}
/// <summary>
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
/// </summary>
/// <param name="b">The b.</param>
/// <returns></returns>
public FPVector Solve33(FPVector b)
{
FP det = FPVector.Dot(ex, FPVector.Cross(ey, ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
return(new FPVector(det * FPVector.Dot(b, FPVector.Cross(ey, ez)), det * FPVector.Dot(ex, FPVector.Cross(b, ez)), det * FPVector.Dot(ex, FPVector.Cross(ey, b))));
}
// sort cached points so most isolated points come first
private int SortCachedPoints(ref FPVector realRelPos1, FP pen)
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
int maxPenetrationIndex = -1;
FP maxPenetration = pen;
for (int i = 0; i < 4; i++)
{
if (contactList[i].penetration > maxPenetration)
{
maxPenetrationIndex = i;
maxPenetration = contactList[i].penetration;
}
}
FP res0 = 0, res1 = 0, res2 = 0, res3 = 0;
if (maxPenetrationIndex != 0)
{
FPVector a0; FPVector.Subtract(ref realRelPos1, ref contactList[1].relativePos1, out a0);
FPVector b0; FPVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
FPVector cross; FPVector.Cross(ref a0, ref b0, out cross);
res0 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 1)
{
FPVector a0; FPVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
FPVector b0; FPVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
FPVector cross; FPVector.Cross(ref a0, ref b0, out cross);
res1 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 2)
{
FPVector a0; FPVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
FPVector b0; FPVector.Subtract(ref contactList[3].relativePos1, ref contactList[1].relativePos1, out b0);
FPVector cross; FPVector.Cross(ref a0, ref b0, out cross);
res2 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 3)
{
FPVector a0; FPVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
FPVector b0; FPVector.Subtract(ref contactList[2].relativePos1, ref contactList[1].relativePos1, out b0);
FPVector cross; FPVector.Cross(ref a0, ref b0, out cross);
res3 = cross.sqrMagnitude;
}
int biggestarea = MaxAxis(res0, res1, res2, res3);
return(biggestarea);
}
/// <summary>
/// 改变朝向，改变forward的朝向跟摄像机的朝向一样
/// </summary>
/// <param name="forward"></param>
public void ChangeAvatarForward(FPVector toForward)
{
JoystickAngle = 90;
if (AvatarForm == E_AvatarForm.PERSON_STATE)
{
_ufpTransformChildObj.ChangeForward(toForward);
return;
}
RaycastHit _groundHit;
_characterMotionObj.RayGroundInfo(out _groundHit);
FPVector _fpNormal = _ufpTransformChildObj.ChangeVec3ToTSVec(_groundHit.normal);
if (_groundHit.normal == Vector3.up)
{
_ufpTransformChildObj.ChangeForward(toForward);
// _childObj.forward = toForward;
JoystickAngle = 90;
return;
}
else
{
FPVector left = FPVector.Cross(toForward, _fpNormal); //切线
FPVector newForward = FPVector.Cross(_fpNormal, left);
if (_fpNormal == FPVector.zero)
{
Debug.Log("forward:::" + toForward);
return;
}
FPQuaternion newRotation = FPQuaternion.LookRotation(newForward, _fpNormal);
U_FPTransform ComMoveTransform = ComMoveFollowObj.GetFPTransform();
FPQuaternion comObject = new FPQuaternion(ComMoveTransform.rotation.x, ComMoveTransform.rotation.y,
ComMoveTransform.rotation.z, ComMoveTransform.rotation.w);
// ComMoveFollowObj.transform.rotation = newRotation;
ComMoveTransform.SetRotation(newRotation);
FPVector comForward = ComMoveTransform.forward;
FPVector childForward = _ufpTransformChildObj.forward;
FP DragAngle = FPVector.Angle(comForward, childForward);
DragAngle = FPMath.Sign(FPVector.Cross(childForward, comForward).y) * DragAngle;
ChangeAvaterForward(DragAngle); //改变对象的旋转数值
ComMoveTransform.SetRotation(comObject); //恢复摄像机原来的rotation数值
}
}
/// <summary>
/// Applies an impulse on the specific position. Changing linear
/// and angular velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
/// <param name="relativePosition">The position where the impulse gets applied
/// in Body coordinate frame.</param>
public void ApplyImpulse(FPVector impulse, FPVector relativePosition)
{
if (this.isStatic)
{
return;
}
FPVector temp;
FPVector.Multiply(ref impulse, inverseMass, out temp);
FPVector.Add(ref linearVelocity, ref temp, out linearVelocity);
FPVector.Cross(ref relativePosition, ref impulse, out temp);
FPVector.Transform(ref temp, ref invInertiaWorld, out temp);
FPVector.Add(ref angularVelocity, ref temp, out angularVelocity);
}
/// Test if point p and d lie on opposite sides of plane through abc
public int PointOutsideOfPlane(FPVector p, FPVector a, FPVector b, FPVector c, FPVector d)
{
FPVector normal = FPVector.Cross(b - a, c - a);
FP signp = FPVector.Dot(p - a, normal); // [AP AB AC]
FP signd = FPVector.Dot(d - a, normal); // [AD AB AC]
//if (CatchDegenerateTetrahedron)
if (signd * signd < (FP.EN8))
{
return(-1);
}
// Points on opposite sides if expression signs are opposite
return(signp * signd < FP.Zero ? 1 : 0);
}
public static void LookAt(FPVector forward, FPVector upwards, out FPMatrix result)
{
FPVector zaxis = forward; zaxis.Normalize();
FPVector xaxis = FPVector.Cross(upwards, zaxis); xaxis.Normalize();
FPVector yaxis = FPVector.Cross(zaxis, xaxis);
result.M11 = xaxis.x;
result.M21 = yaxis.x;
result.M31 = zaxis.x;
result.M12 = xaxis.y;
result.M22 = yaxis.y;
result.M32 = zaxis.y;
result.M13 = xaxis.z;
result.M23 = yaxis.z;
result.M33 = zaxis.z;
}
/// <summary>
/// 跨面操作
/// </summary>
/// <param name="forward"></param>
/// <param name="position"></param>
/// <param name="normal"></param>
public void FitSurfaceParent(FPVector forward, FPVector position, FPVector normal)
{
FPVector left = FPVector.Cross(forward, normal); //切线
FPVector newForward = FPVector.Cross(normal, left);
if (normal == FPVector.zero)
{
Debug.Log("forward:::" + forward);
return;
}
FPQuaternion newRotation = FPQuaternion.LookRotation(newForward, normal);
UpdateRotation(newRotation);
UpdatePosition(position);
//Debug.Log(" _ufpTransform.up::" + _ufpTransform.up.ToVector() + "newRotation::" + newRotation.ToQuaternion()
// + ",forward::" + forward.ToVector() + ",normal:::" + normal.ToVector() + ",newForward::" + newForward.ToVector());
}
/// <summary>
/// Sets the current shape. First <see cref="Prepare"/> has to be called.
/// After SetCurrentShape the shape immitates another shape.
/// </summary>
/// <param name="index"></param>
public override void SetCurrentShape(int index)
{
bool leftTriangle = false;
if (index >= numX * numZ)
{
leftTriangle = true;
index -= numX * numZ;
}
int quadIndexX = index % numX;
int quadIndexZ = index / numX;
// each quad has two triangles, called 'leftTriangle' and !'leftTriangle'
if (leftTriangle)
{
points[0].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[2].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
else
{
points[0].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
points[2].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
FPVector sum = points[0];
FPVector.Add(ref sum, ref points[1], out sum);
FPVector.Add(ref sum, ref points[2], out sum);
FPVector.Multiply(ref sum, FP.One / (3 * FP.One), out sum);
geomCen = sum;
FPVector.Subtract(ref points[1], ref points[0], out sum);
FPVector.Subtract(ref points[2], ref points[0], out normal);
FPVector.Cross(ref sum, ref normal, out normal);
}
/// <summary>
/// Sets the current shape. First <see cref="Prepare"/> has to be called.
/// After SetCurrentShape the shape immitates another shape.
/// </summary>
/// <param name="index"></param>
public override void SetCurrentShape(int index)
{
vecs[0] = octree.GetVertex(octree.tris[potentialTriangles[index]].I0);
vecs[1] = octree.GetVertex(octree.tris[potentialTriangles[index]].I1);
vecs[2] = octree.GetVertex(octree.tris[potentialTriangles[index]].I2);
FPVector sum = vecs[0];
FPVector.Add(ref sum, ref vecs[1], out sum);
FPVector.Add(ref sum, ref vecs[2], out sum);
FPVector.Multiply(ref sum, FP.One / (3 * FP.One), out sum);
geomCen = sum;
FPVector.Subtract(ref vecs[1], ref vecs[0], out sum);
FPVector.Subtract(ref vecs[2], ref vecs[0], out normal);
FPVector.Cross(ref sum, ref normal, out normal);
if (flipNormal)
{
normal.Negate();
}
}
private void OnCollisionEnter(Collision collision)
{
if (!bMoveFlag && _ufpTransform)
{
RaycastHit _groundHit;
if (Physics.Raycast(RayPointObj.transform.position, -Vector3.up, out _groundHit, GroundRayLength)) //悬空
{
FPVector fpNormal = _ufpTransform.ChangeVec3ToTSVec(_groundHit.normal);
if (_ufpTransform.up == fpNormal)
{
return;
}
FP angle = FPVector.Angle(_ufpTransform.up, fpNormal);
FPVector fpPoint = _ufpTransform.ChangeVec3ToTSVec(_groundHit.point);
FPVector newCrossVec3 = FPVector.Cross(_ufpTransform.up, fpNormal);
if (_childObj)
{
FPVector forward = FPQuaternion.AngleAxis(angle, newCrossVec3) * (_ufpTransform.forward);
FitSurfaceParent(forward, fpPoint, fpNormal);
}
}
}
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(FP timestep)
{
FPVector dv = CalculateRelativeVelocity();
FP kNormal = FP.Zero;
FPVector rantra = FPVector.zero;
if (!treatBody1AsStatic)
{
kNormal += body1.inverseMass;
if (!body1IsMassPoint)
{
FPVector.Cross(ref relativePos1, ref normal, out rantra);
FPVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FPVector.Cross(ref rantra, ref relativePos1, out rantra);
}
}
FPVector rbntrb = FPVector.zero;
if (!treatBody2AsStatic)
{
kNormal += body2.inverseMass;
if (!body2IsMassPoint)
{
FPVector.Cross(ref relativePos2, ref normal, out rbntrb);
FPVector.Transform(ref rbntrb, ref body2.invInertiaWorld, out rbntrb);
FPVector.Cross(ref rbntrb, ref relativePos2, out rbntrb);
}
}
if (!treatBody1AsStatic)
{
kNormal += FPVector.Dot(ref rantra, ref normal);
}
if (!treatBody2AsStatic)
{
kNormal += FPVector.Dot(ref rbntrb, ref normal);
}
massNormal = FP.One / kNormal;
tangent = dv - FPVector.Dot(dv, normal) * normal;
tangent.Normalize();
FP kTangent = FP.Zero;
if (treatBody1AsStatic)
{
rantra.MakeZero();
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
FPVector.Cross(ref relativePos1, ref normal, out rantra);
FPVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FPVector.Cross(ref rantra, ref relativePos1, out rantra);
}
}
if (treatBody2AsStatic)
{
rbntrb.MakeZero();
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
FPVector.Cross(ref relativePos2, ref tangent, out rbntrb);
FPVector.Transform(ref rbntrb, ref body2.invInertiaWorld, out rbntrb);
FPVector.Cross(ref rbntrb, ref relativePos2, out rbntrb);
}
}
if (!treatBody1AsStatic)
{
kTangent += FPVector.Dot(ref rantra, ref tangent);
}
if (!treatBody2AsStatic)
{
kTangent += FPVector.Dot(ref rbntrb, ref tangent);
}
massTangent = FP.One / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = FP.Zero;
FP relNormalVel = FPVector.Dot(ref normal, ref dv);
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (FP.One / timestep) * FPMath.Max(FP.Zero, Penetration - settings.allowedPenetration);
restitutionBias = FPMath.Clamp(restitutionBias, FP.Zero, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
FP timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
FP relTangentVel = -FPVector.Dot(ref tangent, ref dv);
FP tangentImpulse = massTangent * relTangentVel;
FP maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
FPVector impulse;
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
if (relNormalVel < -FP.One && newContact)
{
restitutionBias = FPMath.Max(-restitution * relNormalVel, restitutionBias);
}
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = FP.Zero;
}
else
{
lostSpeculativeBounce = FP.Zero;
}
impulse = normal * accumulatedNormalImpulse + tangent * accumulatedTangentImpulse;
ApplyImpulse(ref impulse);
lastTimeStep = timestep;
newContact = false;
}
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));
}
}
}
/// <summary>
/// Checks two shapes for collisions.
/// </summary>
/// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
/// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
/// <param name="orientation1">The orientation of the first shape.</param>
/// <param name="orientation2">The orientation of the second shape.</param>
/// <param name="position1">The position of the first shape.</param>
/// <param name="position2">The position of the second shape</param>
/// <param name="point">The pointin world coordinates, where collision occur.</param>
/// <param name="normal">The normal pointing from body2 to body1.</param>
/// <param name="penetration">Estimated penetration depth of the collision.</param>
/// <returns>Returns true if there is a collision, false otherwise.</returns>
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref FPMatrix orientation1,
ref FPMatrix orientation2, ref FPVector position1, ref FPVector position2,
out FPVector point, out FPVector normal, out FP penetration)
{
// Used variables
FPVector temp1, temp2;
FPVector v01, v02, v0;
FPVector v11, v12, v1;
FPVector v21, v22, v2;
FPVector v31, v32, v3;
FPVector v41 = FPVector.zero, v42 = FPVector.zero, v4 = FPVector.zero;
FPVector mn;
// Initialization of the output
point = normal = FPVector.zero;
penetration = FP.Zero;
//JVector right = JVector.Right;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
FPVector.Transform(ref v01, ref orientation1, out v01);
FPVector.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
FPVector.Transform(ref v02, ref orientation2, out v02);
FPVector.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
FPVector.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.IsNearlyZero())
{
v0 = new FPVector(FP.EN4, 0, 0);
}
// v1 = support in direction of origin
mn = v0;
FPVector.Negate(ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
FPVector.Subtract(ref v12, ref v11, out v1);
if (FPVector.Dot(ref v1, ref normal) <= FP.Zero)
{
return(false);
}
// v2 = support perpendicular to v1,v0
FPVector.Cross(ref v1, ref v0, out normal);
if (normal.IsNearlyZero())
{
FPVector.Subtract(ref v1, ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
normal.Normalize();
point = v11;
FPVector.Add(ref point, ref v12, out point);
FPVector.Multiply(ref point, FP.Half, out point);
FPVector.Subtract(ref v12, ref v11, out temp1);
penetration = FPVector.Dot(ref temp1, ref normal);
//point = v11;
//point2 = v12;
return(true);
}
FPVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
FPVector.Subtract(ref v22, ref v21, out v2);
if (FPVector.Dot(ref v2, ref normal) <= FP.Zero)
{
return(false);
}
// Determine whether origin is on + or - side of plane (v1,v0,v2)
FPVector.Subtract(ref v1, ref v0, out temp1);
FPVector.Subtract(ref v2, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
FP dist = FPVector.Dot(ref normal, ref v0);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > FP.Zero)
{
FPVector.Swap(ref v1, ref v2);
FPVector.Swap(ref v11, ref v21);
FPVector.Swap(ref v12, ref v22);
FPVector.Negate(ref normal, out normal);
UnityEngine.Debug.Log("normal: " + normal);
}
int phase2 = 0;
int phase1 = 0;
bool hit = false;
// Phase One: Identify a portal
while (true)
{
if (phase1 > MaximumIterations)
{
return(false);
}
phase1++;
// Obtain the support point in a direction perpendicular to the existing plane
// Note: This point is guaranteed to lie off the plane
FPVector.Negate(ref normal, out mn);
//UnityEngine.Debug.Log("mn: " + mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
FPVector.Subtract(ref v32, ref v31, out v3);
if (FPVector.Dot(ref v3, ref normal) <= FP.Zero)
{
return(false);
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
FPVector.Cross(ref v1, ref v3, out temp1);
if (FPVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v2 = v3;
v21 = v31;
v22 = v32;
FPVector.Subtract(ref v1, ref v0, out temp1);
FPVector.Subtract(ref v3, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
// UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
FPVector.Cross(ref v3, ref v2, out temp1);
if (FPVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v1 = v3;
v11 = v31;
v12 = v32;
FPVector.Subtract(ref v3, ref v0, out temp1);
FPVector.Subtract(ref v2, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
phase2++;
/*
* UnityEngine.Debug.LogError(" ::Start STATE");
* UnityEngine.Debug.Log(temp1 + " " + temp2);
* UnityEngine.Debug.Log( v01 + " " + v02 + " "+ v0);
* UnityEngine.Debug.Log( v11+" "+ v12 +" "+ v1);
* UnityEngine.Debug.Log( v21 +" "+ v22 +" "+ v2);
* UnityEngine.Debug.Log( v31 +" "+ v32 +" "+ v3);
* UnityEngine.Debug.Log( v41 +" "+ v42 +" "+ v4);
* UnityEngine.Debug.Log( mn);
*
* UnityEngine.Debug.LogError(" ::END STATE");
*/
// Compute normal of the wedge face
FPVector.Subtract(ref v2, ref v1, out temp1);
FPVector.Subtract(ref v3, ref v1, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
// Beginer
// UnityEngine.Debug.Log("normal: " + normal);
// Can this happen??? Can it be handled more cleanly?
if (normal.IsNearlyZero())
{
return(true);
}
normal.Normalize();
//UnityEngine.Debug.Log("normal: " + normal);
// Compute distance from origin to wedge face
FP d = FPVector.Dot(ref normal, ref v1);
// If the origin is inside the wedge, we have a hit
if (d >= 0 && !hit)
{
// HIT!!!
hit = true;
}
// Find the support point in the direction of the wedge face
FPVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
FPVector.Subtract(ref v42, ref v41, out v4);
FPVector.Subtract(ref v4, ref v3, out temp1);
FP delta = FPVector.Dot(ref temp1, ref normal);
penetration = FPVector.Dot(ref v4, ref normal);
// If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
if (delta <= CollideEpsilon || penetration <= FP.Zero || phase2 > MaximumIterations)
{
if (hit)
{
FPVector.Cross(ref v1, ref v2, out temp1);
FP b0 = FPVector.Dot(ref temp1, ref v3);
FPVector.Cross(ref v3, ref v2, out temp1);
FP b1 = FPVector.Dot(ref temp1, ref v0);
FPVector.Cross(ref v0, ref v1, out temp1);
FP b2 = FPVector.Dot(ref temp1, ref v3);
FPVector.Cross(ref v2, ref v1, out temp1);
FP b3 = FPVector.Dot(ref temp1, ref v0);
FP sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
FPVector.Cross(ref v2, ref v3, out temp1);
b1 = FPVector.Dot(ref temp1, ref normal);
FPVector.Cross(ref v3, ref v1, out temp1);
b2 = FPVector.Dot(ref temp1, ref normal);
FPVector.Cross(ref v1, ref v2, out temp1);
b3 = FPVector.Dot(ref temp1, ref normal);
sum = b1 + b2 + b3;
}
FP inv = FP.One / sum;
FPVector.Multiply(ref v01, b0, out point);
FPVector.Multiply(ref v11, b1, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v21, b2, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v31, b3, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v02, b0, out temp2);
FPVector.Add(ref temp2, ref point, out point);
FPVector.Multiply(ref v12, b1, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v22, b2, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v32, b3, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref point, inv * FP.Half, out point);
}
// Compute the barycentric coordinates of the origin
return(hit);
}
//// Compute the tetrahedron dividing face (v4,v0,v1)
//JVector.Cross(ref v4, ref v1, out temp1);
//FP d1 = JVector.Dot(ref temp1, ref v0);
//// Compute the tetrahedron dividing face (v4,v0,v2)
//JVector.Cross(ref v4, ref v2, out temp1);
//FP d2 = JVector.Dot(ref temp1, ref v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
//UnityEngine.Debug.LogError("v4:" + v4 + " v0:" + v0);
FPVector.Cross(ref v4, ref v0, out temp1);
//UnityEngine.Debug.LogError("temp1:"+ temp1);
//Ender
// UnityEngine.Debug.Log("normal: " + normal);
FP dot = FPVector.Dot(ref temp1, ref v1);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 temp1:" + temp1 + " v2:" + v2 );
dot = FPVector.Dot(ref temp1, ref v2);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v1->v4");
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < v3->v4");
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
// UnityEngine.Debug.Log("dot < 0 temp1:" + temp1 + " v3:" + v3 );
dot = FPVector.Dot(ref temp1, ref v3);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v2 => v4");
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < 0 v1 => v4");
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}
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;
}
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;
}
}
}
}