public static FixedPointF Sqrt(FixedPointF t)
{
double v = (double)t.m_value / (1 << m_float_num);
double d = Math.Sqrt(v);
FixedPointF f = new FixedPointF(d);
return(f);
}
public override bool Equals(object obj)
{
if (obj is FixedPointF)
{
FixedPointF f = (FixedPointF)obj;
return(f.m_value == m_value);
}
return(false);
}
public override void DrawGizmos(Color tColor)
{
#if UNITY_EDITOR
FixedPointF radius = mRadius;
Vector3 pos = mTrans.Position.value;
Gizmos.color = tColor;
Handles.color = tColor;
Handles.CircleHandleCap(0, pos, Quaternion.Euler(90, 0, 0), radius.value, EventType.Repaint);
#endif
}
static void Push(CustomCollider a, CustomCollider b, FixedPointF offsetfac, CustomVector2 offsetvec, bool a_at_left)
{
FixedPointF fac = offsetfac;
CustomVector3 offset_pos;
offset_pos.x = fac * offsetvec.x;
offset_pos.y = new FixedPointF(0);
offset_pos.z = fac * offsetvec.y;
//Actor actor = null;
//bool actorisA = false;
//if (a.mIsobstacle && !b.mIsobstacle)
// actor = b.GetComponentInParent<Actor>();
//else if (!a.mIsobstacle && b.mIsobstacle)
//{
// actor = a.GetComponentInParent<Actor>();
// actorisA = true;
//}
//if (actor != null)
//{
// if (a_at_left == actorisA)
// actor.Position -= offset_pos;
// else
// actor.Position += offset_pos;
//}
if (!a.mIsobstacle && !b.mIsobstacle)
{
return;
}
if (a.mIsobstacle && !b.mIsobstacle)
{
if (a_at_left)
{
b.AddOffsetPosition(offset_pos);
}
else
{
b.AddOffsetPosition(-offset_pos);
}
}
else if (!a.mIsobstacle && b.mIsobstacle)
{
if (a_at_left)
{
a.AddOffsetPosition(-offset_pos);
}
else
{
a.AddOffsetPosition(offset_pos);
}
}
}
// 移动罗盘只控制方向，不控制速度
public void Move()
{
CustomVector3 temp;
FixedPointF x = CustomMath.GetCos(_RotationTf.Angle);
FixedPointF z = CustomMath.GetSin(_RotationTf.Angle);
temp.x = x * _MoveSpeed * ConstValue._RenderFrameRate;
temp.y = new FixedPointF(0);
temp.z = z * _MoveSpeed * ConstValue._RenderFrameRate;
_PositionTf.LocalPosition += temp;
}
/// <summary>
/// 世界坐标转本地坐标，复合变换
/// </summary>
public CustomVector3 WorldToLocal(CustomVector3 worldPos)
{
CustomVector3 tOffset = mTrans.Position;
worldPos.x -= tOffset.x;
worldPos.z -= tOffset.z;
FixedPointF cos = CustomMath.GetCos(-mTrans.Angle);
FixedPointF sin = CustomMath.GetSin(-mTrans.Angle);
CustomVector3 vec2 = mTrans.Position;
vec2.x = worldPos.x * cos - worldPos.z * sin;
vec2.z = worldPos.z * cos + worldPos.x * sin;
return(vec2);
}
public virtual void Move()
{
CustomTransform mTrans = GetComponent <CustomTransform>();
CustomTransform mRotateTrans = RotateTransform.GetComponent <CustomTransform>();
CustomVector3 temp;
FixedPointF x = CustomMath.GetCos(mRotateTrans.Angle);
FixedPointF z = CustomMath.GetSin(mRotateTrans.Angle);
temp.x = x * Speed * LockStepConfig.mRenderFrameRate;
temp.y = new FixedPointF(0);
temp.z = z * Speed * LockStepConfig.mRenderFrameRate;
mTrans.LocalPosition += temp;
}
/// <summary>
/// 本地坐标转世界坐标，复合变换
/// </summary>
public CustomVector3 LocalToWorld(CustomVector3 localPos)
{
CustomVector3 vec = localPos;
FixedPointF cos = CustomMath.GetCos(mTrans.Angle);
FixedPointF sin = CustomMath.GetSin(mTrans.Angle);
CustomVector3 vec2 = localPos;
vec2.x = vec.x * cos - vec.z * sin;
vec2.z = vec.z * cos + vec.x * sin;
CustomVector3 tOffset = mTrans.Position;
vec2.x += tOffset.x;
vec2.z += tOffset.z;
return(vec2);
}
// 遥感获得角度范围是-180到180
public static FixedPointF GetCos(int tAngle)
{
// -180
if (tAngle == -180)
{
return(new FixedPointF(-1));
}
// -179和-89
else if (tAngle < -90)
{
tAngle = -tAngle;
tAngle = 180 - tAngle;
FixedPointF cos = Cos[tAngle];
cos = -cos;
return(cos);
}
// -90到-1
else if (tAngle < 0)
{
tAngle = -tAngle;
return(Cos[tAngle]);
}
// 0到89
else if (tAngle < 90)
{
return(Cos[tAngle]);
}
// 90到179
else if (tAngle < 180)
{
tAngle = 180 - tAngle;
FixedPointF cos = Cos[tAngle];
cos = -cos;
return(cos);
}
// 180
else
{
return(new FixedPointF(-1));
}
}
// 遥感获得角度范围是-180到180
public static FixedPointF GetSin(int tAngle)
{
// -180
if (tAngle == -180)
{
return(new FixedPointF(0));
}
// -179和-89
else if (tAngle < -90)
{
tAngle = -tAngle;
tAngle = 180 - tAngle;
FixedPointF sin = Sin[tAngle];
sin = -sin;
return(sin);
}
// -90到-1
else if (tAngle < 0)
{
tAngle = -tAngle;
FixedPointF sin = Sin[tAngle];
sin = -sin;
return(sin);
}
// 0到89
else if (tAngle < 90)
{
return(Sin[tAngle]);
}
// 90到179
else if (tAngle < 180)
{
tAngle = 180 - tAngle;
return(Sin[tAngle]);
}
// 180
else
{
return(new FixedPointF(0));
}
}
/// <summary>
/// 获得多边形在一条直线上的投影范围
/// </summary>
/// <param name="list"></param>
/// <param name="axis"></param>
/// <returns></returns>
static CustomVector2 GetMapPointMinMaxDis(List <CustomVector3> list, CustomVector2 axis)
{
CustomVector2 point = new CustomVector2(list[0].x, list[0].z);
FixedPointF min = CustomVector2.Dot(point, axis);
FixedPointF max = min;
for (int i = 1; i < list.Count; i++)
{
point = new CustomVector2(list[i].x, list[i].z);
FixedPointF doc = CustomVector2.Dot(point, axis);
if (doc < min)
{
min = doc;
}
if (doc > max)
{
max = doc;
}
}
point.x = min;
point.y = max;
return(point);
}
/// <summary>
/// 设置推开向量
/// </summary>
/// <param name="proj_a"></param>
/// <param name="proj_b"></param>
/// <param name="a_at_left_temp"></param>
/// <param name="offsetfac_temp"></param>
static void Set_PushVec(CustomVector2 proj_a, CustomVector2 proj_b, ref bool a_at_left_temp, ref FixedPointF offsetfac_temp)
{
if (proj_a.x == proj_b.x)
{
if (proj_a.y <= proj_b.y)
{
a_at_left_temp = true;
offsetfac_temp = proj_a.y - proj_a.x;
}
else
{
a_at_left_temp = false;
offsetfac_temp = proj_b.y - proj_a.x;
}
}
else if (proj_a.x < proj_b.x)
{
a_at_left_temp = true;
offsetfac_temp = proj_a.y - proj_b.x;
}
else
{
a_at_left_temp = false;
offsetfac_temp = proj_b.y - proj_a.x;
}
}
protected override void Reset()
{
base.Reset();
Width = new FixedPointF(1);
Height = new FixedPointF(1);
}
public CustomVector3(int tX, int tY, int tZ)
{
x = new FixedPointF(tX);
y = new FixedPointF(tY);
z = new FixedPointF(tZ);
}
public CustomVector3(FixedPointF tX, FixedPointF tY, FixedPointF tZ)
{
x = tX;
y = tY;
z = tZ;
}
public CustomVector2(FixedPointF tX, int tY)
{
x = tX;
y = new FixedPointF(tY);
}
public static FixedPointF operator /(FixedPointF t1, int t2)
{
FixedPointF temp = t1 / new FixedPointF(t2);
return(temp);
}
public CustomVector2(FixedPointF tX, FixedPointF tY)
{
x = tX;
y = tY;
}
public void SetData(CustomTransform position, CustomTransform rotation, FixedPointF speed)
{
_PositionTf = position;
_RotationTf = rotation;
_MoveSpeed = speed;
}
/// <summary>
/// 检查两个凸多边形是否重合
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static bool CheckPolygonAndPolygon(CustomPolygonCollider a, CustomPolygonCollider b)
{
//障碍物之间的碰撞不处理
if (a.mIsobstacle && b.mIsobstacle)
{
return(false);
}
List <CustomVector3> a_worldBound = a.LocalToWorldBound;
List <CustomVector3> b_worldBound = b.LocalToWorldBound;
bool mInit = false;
//a在左
bool a_at_left = true;
//偏移向量
CustomVector2 offsetvec = new CustomVector2(0, 0);
//偏移值
FixedPointF offsetfac = new FixedPointF(0);
//顶点连线，
//斜率k
List <CustomVector2> a_edges = new List <CustomVector2>();
for (int i = 0; i < a_worldBound.Count; i++)
{
int point1_index = i;
int point2_index = (i + 1) % a_worldBound.Count;
CustomVector3 offset = a_worldBound[point2_index] - a_worldBound[point1_index];
a_edges.Add(new CustomVector2(offset.x, offset.z));
}
for (int i = 0; i < a_edges.Count; i++)
{
CustomVector2 axis = a_edges[i];
//获得法向量
axis = Vec_normal(axis);
FixedPointF x = axis.x;
FixedPointF y = axis.y;
FixedPointF temp = x * x + y * y;
FixedPointF z = FixedPointF.Sqrt(temp);
axis.x = x / z;
axis.y = y / z;
CustomVector2 proj_a = GetMapPointMinMaxDis(a_worldBound, axis), proj_b = GetMapPointMinMaxDis(b_worldBound, axis);
if (!Check_Overlap(proj_a, proj_b))
{
return(false);
}
else
{
bool a_at_left_temp = false;
FixedPointF offsetfac_temp = new FixedPointF(0);
Set_PushVec(proj_a, proj_b, ref a_at_left_temp, ref offsetfac_temp);
if (!mInit)
{
a_at_left = a_at_left_temp;
offsetfac = offsetfac_temp;
offsetvec = axis;
mInit = true;
}
else
{
if (offsetfac_temp < offsetfac)
{
a_at_left = a_at_left_temp;
offsetfac = offsetfac_temp;
offsetvec = axis;
}
}
}
}
//顶点连线
//斜率k
List <CustomVector2> b_edges = new List <CustomVector2>();
for (int i = 0; i < b_worldBound.Count; i++)
{
int point1_index = i;
int point2_index = (i + 1) % b_worldBound.Count;
CustomVector3 offset = b_worldBound[point2_index] - b_worldBound[point1_index];
b_edges.Add(new CustomVector2(offset.x, offset.z));
}
for (int i = 0; i < b_edges.Count; i++)
{
CustomVector2 axis = b_edges[i];
axis = Vec_normal(axis);
FixedPointF x = axis.x;
FixedPointF y = axis.y;
FixedPointF temp = x * x + y * y;
FixedPointF z = FixedPointF.Sqrt(temp);
axis.x = x / z;
axis.y = y / z;
CustomVector2 proj_a = GetMapPointMinMaxDis(a_worldBound, axis), proj_b = GetMapPointMinMaxDis(b_worldBound, axis);
if (!Check_Overlap(proj_a, proj_b))
{
return(false);
}
{
bool a_at_left_temp = false;
FixedPointF offsetfac_temp = new FixedPointF(0);
Set_PushVec(proj_a, proj_b, ref a_at_left_temp, ref offsetfac_temp);
if (offsetfac_temp < offsetfac)
{
a_at_left = a_at_left_temp;
offsetfac = offsetfac_temp;
offsetvec = axis;
}
}
}
Push(a, b, offsetfac, offsetvec, a_at_left);
return(true);
}
public CustomVector2(int tX, FixedPointF tY)
{
x = new FixedPointF(tX);
y = tY;
}
/// <summary>
/// 检查凸多边形和圆是否重合
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static bool CheckPolygonAndCircle(CustomPolygonCollider a, CustomCircleCollider b)
{
//障碍物之间的碰撞不处理
if (a.mIsobstacle && b.mIsobstacle)
{
return(false);
}
List <CustomVector3> a_worldBound = a.LocalToWorldBound;
bool mInit = false;
//a在左
bool a_at_left = true;
//偏移向量
CustomVector2 offsetvec = new CustomVector2(0, 0);
//偏移值
FixedPointF offsetfac = new FixedPointF(0);
//顶点连线
//斜率k
List <CustomVector2> a_edges = new List <CustomVector2>();
for (int i = 0; i < a_worldBound.Count; i++)
{
int point1_index = i;
int point2_index = (i + 1) % a_worldBound.Count;
CustomVector3 offset = a_worldBound[point2_index] - a_worldBound[point1_index];
a_edges.Add(new CustomVector2(offset.x, offset.z));
}
for (int i = 0; i < a_edges.Count; i++)
{
CustomVector2 axis = a_edges[i];
axis = Vec_normal(axis);
FixedPointF x = axis.x;
FixedPointF y = axis.y;
FixedPointF temp = x * x + y * y;
FixedPointF z = FixedPointF.Sqrt(temp);
axis.x = x / z;
axis.y = y / z;
//求圆在法线上的投影边界
CustomVector3 vec3 = b.mTrans.Position;
CustomVector2 point = new CustomVector2(vec3.x, vec3.z);
FixedPointF mapPoint = CustomVector2.Dot(point, axis);
FixedPointF min = mapPoint - b.mRadius;
FixedPointF max = mapPoint + b.mRadius;
CustomVector2 proj_b = new CustomVector2(min, max);
CustomVector2 proj_a = GetMapPointMinMaxDis(a_worldBound, axis);
if (!Check_Overlap(proj_a, proj_b))
{
return(false);
}
else
{
bool a_at_left_temp = false;
FixedPointF offsetfac_temp = new FixedPointF(0);
Set_PushVec(proj_a, proj_b, ref a_at_left_temp, ref offsetfac_temp);
if (!mInit)
{
a_at_left = a_at_left_temp;
offsetfac = offsetfac_temp;
offsetvec = axis;
mInit = true;
}
else
{
if (offsetfac_temp < offsetfac)
{
a_at_left = a_at_left_temp;
offsetfac = offsetfac_temp;
offsetvec = axis;
}
}
}
}
Push(a, b, offsetfac, offsetvec, a_at_left);
return(true);
}
/// <summary>
/// 检查凸两个圆是否重合
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static bool CheckCircleAndCircle(CustomCircleCollider a, CustomCircleCollider b)
{
//障碍物之间的碰撞不处理
if (a.mIsobstacle && b.mIsobstacle)
{
return(false);
}
bool mInit = false;
//a在左
bool a_at_left = true;
//偏移向量
CustomVector2 offsetvec = new CustomVector2(0, 0);
//偏移值
FixedPointF offsetfac = new FixedPointF(0);
//圆心连线
CustomVector3 vec3 = a.mTrans.Position - b.mTrans.Position;
CustomVector2 axis = new CustomVector2(vec3.x, vec3.z);
FixedPointF x = axis.x;
FixedPointF y = axis.y;
FixedPointF temp = x * x + y * y;
FixedPointF z = FixedPointF.Sqrt(temp);
//圆心重合，x轴推开
if (z == FixedPointF.zero)
{
axis.x = new FixedPointF(1000, 1000);
axis.y = new FixedPointF(0, 1000);
}
else
{
axis.x = x / z;
axis.y = y / z;
}
CustomVector3 apos = a.mTrans.Position;
CustomVector2 point = new CustomVector2(apos.x, apos.z);
FixedPointF mapPoint = CustomVector2.Dot(point, axis);
FixedPointF min = mapPoint - a.mRadius;
FixedPointF max = mapPoint + a.mRadius;
CustomVector2 proj_a = new CustomVector2(min, max);
CustomVector3 bpos = b.mTrans.Position;
point = new CustomVector2(bpos.x, bpos.z);
mapPoint = CustomVector2.Dot(point, axis);
min = mapPoint - b.mRadius;
max = mapPoint + b.mRadius;
CustomVector2 proj_b = new CustomVector2(min, max);
if (!Check_Overlap(proj_a, proj_b))
{
return(false);
}
else
{
bool a_at_left_temp = false;
FixedPointF offsetfac_temp = new FixedPointF(0);
Set_PushVec(proj_a, proj_b, ref a_at_left_temp, ref offsetfac_temp);
if (!mInit)
{
a_at_left = a_at_left_temp;
offsetfac = offsetfac_temp;
offsetvec = axis;
mInit = true;
}
else
{
if (offsetfac_temp < offsetfac)
{
a_at_left = a_at_left_temp;
offsetfac = offsetfac_temp;
offsetvec = axis;
}
}
}
Push(a, b, offsetfac, offsetvec, a_at_left);
return(true);
}
protected override void Reset()
{
base.Reset();
mRadius = new FixedPointF(0.5f);
}
public CustomVector2(int tX, int tY)
{
x = new FixedPointF(tX);
y = new FixedPointF(tY);
}
