public static LQuaternion FromToRotation(LVector3 fromDirection, LVector3 toDirection)
{
LFloat angle = LVector3.Angle(fromDirection, toDirection);
LVector3 axis = fromDirection.Cross(toDirection);
return(AngleAxis(angle, axis));
}
public bool Search(out LVector3 found)
{
found = LVector3.Invalid;
if (lookingForBlock || true)
{
RaycastResults res;
for (int i = 0; i < 50; i++)
{
if (PhysicsUtils.CustomRaycast(transform.position, Random.onUnitSphere, 20.0f, (b, bx, by, bz, pbx, pby, pbz) => { return(true); }, (b, bx, by, bz, pbx, pby, pbz) =>
{
return(DesiredBlock(b));
}, out res))
{
found = res.hitBlock;
return(true);
}
}
}
else
{
Debug.Log("not looking for block but called search?");
}
return(false);
}
public override void UpdatePosition(LVector3 targetPos)
{
var oldR = r;
min = targetPos - oldR;
max = targetPos + oldR;
}
// Transform AABB a by the matrix m and translation t,
// find maximum extents, and store result into AABB b.
public void UpdateAABB(LMatrix33 m, LVector3 t)
{
LVector3 _c = c + t;
LVector3 _r = r;
min = max = _c;
// For all three axes
for (int i = 0; i < 3; i++)
{
// Form extent by summing smaller and larger terms respectively
for (int j = 0; j < 3; j++)
{
LFloat e = m[i, j] * _r[j];
if (e < LFloat.zero)
{
min[i] += e;
max[i] -= e;
}
else
{
min[i] -= e;
max[i] += e;
}
}
}
}
public void DrawGizmos()
{
//draw children
if (_childA != null)
{
_childA->DrawGizmos();
}
if (_childB != null)
{
_childB->DrawGizmos();
}
if (_childC != null)
{
_childC->DrawGizmos();
}
if (_childD != null)
{
_childD->DrawGizmos();
}
//draw rect
Gizmos.color = Color.cyan;
var p1 = new LVector3(_bounds.position.x, 0.1f.ToLFloat(), _bounds.position.y);
var p2 = new LVector3(p1.x + _bounds.width, 0.1f.ToLFloat(), p1.z);
var p3 = new LVector3(p1.x + _bounds.width, 0.1f.ToLFloat(), p1.z + _bounds.height);
var p4 = new LVector3(p1.x, 0.1f.ToLFloat(), p1.z + _bounds.height);
DrawLine(p1, p2);
DrawLine(p2, p3);
DrawLine(p3, p4);
DrawLine(p4, p1);
}
private void Start()
{
var count = (int)ETestShape3D.EnumCount;
for (int i = 0; i < count; i++)
{
for (int j = 0; j < count; j++)
{
LVector3 pos = new LVector3(true, (i * gridGap), (j * gridGap), 0);
var shape1 = CreateShape(i, (i * count + j) * 2,
pos.ToVector3());
var shape2 = CreateShape(j, (i * count + j) * 2 + 1,
(pos + new LVector3(true, moveDist, 0f, 0f)).ToVector3());
allPairs.Add(new DebugUnityColliderProxy[2] {
shape1, shape2
});
StartCoroutine(
PingPongMove(shape1, shape1.transform.position.ToLVector3(),
pos + new LVector3(true, moveDist, moveDist, 0f), rawMoveTime));
StartCoroutine(
PingPongMove(shape2, shape2.transform.position.ToLVector3(),
pos + new LVector3(true, 0f, moveDist, 0f), rawMoveTime));
}
}
}
// Given point p, return point q on (or in) Rect r, closest to p
public static void ClosestPtPointRect(LVector3 p, Rect r, out LVector3 q)
{
LVector3 d = p - r.c;
// Start result at center of rect; make steps from there
q = r.c;
// For each rect axis...
for (int i = 0; i < 2; i++)
{
// ...project d onto that axis to get the distance
// along the axis of d from the rect center
LFloat dist = Dot(d, r.u[i]);
// If distance farther than the rect extents, clamp to the rect
if (dist > r.e[i])
{
dist = r.e[i];
}
if (dist < -r.e[i])
{
dist = -r.e[i];
}
// Step that distance along the axis to get world coordinate
q += dist * r.u[i];
}
}
// Test if segments ab and cd overlap. If they do, compute and return
// intersection t value along ab and intersection position p
public static bool Test2DSegmentSegment(LVector3 a, LVector3 b, LVector3 c, LVector3 d, ref LFloat t, ref LVector3 p)
{
// Sign of areas correspond to which side of ab points c and d are
LFloat a1 = Signed2DTriArea(a, b, d); // Compute winding of abd (+ or -)
LFloat a2 = Signed2DTriArea(a, b, c); // To intersect, must have sign opposite of a1
// If c and d are on different sides of ab, areas have different signs
if (a1 * a2 < LFloat.zero)
{
// Compute signs for a and b with respect to segment cd
LFloat a3 = Signed2DTriArea(c, d, a); // Compute winding of cda (+ or -)
// Since area is constant a1-a2 = a3-a4, or a4=a3+a2-a1
//LFloat a4 = Signed2DTriArea(c, d, b); // Must have opposite sign of a3
LFloat a4 = a3 + a2 - a1;
// Points a and b on different sides of cd if areas have different signs
if (a3 * a4 < LFloat.zero)
{
// Segments intersect. Find intersection point along L(t)=a+t*(b-a).
// Given height h1 of a over cd and height h2 of b over cd,
// t = h1 / (h1 - h2) = (b*h1/2) / (b*h1/2 - b*h2/2) = a3 / (a3 - a4),
// where b (the base of the triangles cda and cdb, i.e., the length
// of cd) cancels out.
t = a3 / (a3 - a4);
p = a + t * (b - a);
return(true);
}
}
// Segments not intersecting (or collinear)
return(false);
}
public void AddRelativeForce(LVector2 relativeForce, LVector2 localPosition, LForceMode2D forceMode)
{
LVector3 globalForce = transform.rotation * (LVector3)relativeForce;
LVector2 globalPosition = transform.TransformPoint(localPosition);
AddForceAtPosition(globalForce, globalPosition, forceMode);
}
// Given point p, return point q on (or in) OBB b, closest to p
public static void ClosestPtPointOBB(LVector3 p, OBB b, out LVector3 q)
{
LVector3 d = p - b.c;
// Start result at center of box; make steps from there
q = b.c;
// For each OBB axis...
for (int i = 0; i < 3; i++)
{
// ...project d onto that axis to get the distance
// along the axis of d from the box center
LFloat dist = Dot(d, b.u[i]);
// If distance farther than the box extents, clamp to the box
if (dist > b.e[i])
{
dist = b.e[i];
}
if (dist < -b.e[i])
{
dist = -b.e[i];
}
// Step that distance along the axis to get world coordinate
q += dist * b.u[i];
}
}
protected override void OnUpdate()
{
LFloat deltaTime = new LFloat(true, (int)(Time.DeltaTime * LFloat.Precision));
int width = PathfindingManager.Instance.navMap.xCount;
List <NavMapPoint> navMapPoints = PathfindingManager.Instance.navMap.navMapPoints;
Entities.ForEach((Entity entity, DynamicBuffer <PathPosition> pathPositionBuffer, ref LTranslation translation, ref PathFollow pathFollow) => {
if (pathFollow.pathIndex >= 0)
{
// Has path to follow
PathPosition pathPosition = pathPositionBuffer[pathFollow.pathIndex];
int targetIndex = pathPosition.position.x + pathPosition.position.y * width;
LVector3 targetPos = new LVector3(navMapPoints[targetIndex].position.x, 0, navMapPoints[targetIndex].position.y);
LVector3 moveDir = (targetPos - translation.position).normalized;
LFloat moveSpeed = LFloat.one * 5;
translation.position += moveDir * moveSpeed * deltaTime;
if ((translation.position - targetPos).magnitude < LFloat.FromThousand(100))
{
pathFollow.pathIndex--;
}
}
});
}
// Intersect sphere s with movement vector v with plane p. If intersecting
// return time t of collision and point q at which sphere hits plane
public static bool IntersectMovingSpherePlane(Sphere s, LVector3 v, Plane p, out LFloat t, out LVector3 q)
{
// Compute distance of sphere center to plane
LFloat dist = Dot(p.n, s.c) - p.d;
if (Abs(dist) <= s.r)
{
// The sphere is already overlapping the plane. Set time of
// intersection to zero and q to sphere center
t = LFloat.zero;
q = s.c;
return(true);
}
else
{
LFloat denom = Dot(p.n, v);
if (denom * dist >= LFloat.zero)
{
// No intersection as sphere moving parallel to or away from plane
t = LFloat.zero;
q = s.c;
return(false);
}
else
{
// Sphere is moving towards the plane
// Use +r in computations if sphere in front of plane, else -r
LFloat r = dist > LFloat.zero ? s.r : -s.r;
t = (r - dist) / denom;
q = s.c + t * v - r * p.n;
return(true);
}
}
}
// Intersect sphere s0 moving in direction d over time interval t0 <= t <= t1, against
// a stationary sphere s1. If found intersecting, return time t of collision
public static bool TestMovingSphereSphere(Sphere s0, LVector3 d, LFloat t0, LFloat t1, Sphere s1, out LFloat t)
{
// Compute sphere bounding motion of s0 during time interval from t0 to t1
Sphere b = new Sphere();//TODO 移除掉new
LFloat mid = (t0 + t1) * LFloat.half;
b.c = s0.c + d * mid;
b.r = (mid - t0) * d.magnitude + s0.r;
t = LFloat.zero;
// If bounding sphere not overlapping s1, then no collision in this interval
if (!TestSphereSphere(b, s1))
{
return(false);
}
// Cannot rule collision out: recurse for more accurate testing. To terminate the
// recursion, collision is assumed when time interval becomes sufficiently small
if (t1 - t0 < LFloat.INTERVAL_EPSI_LON)
{
t = t0;
return(true);
}
// Recursively test first half of interval; return collision if detected
if (TestMovingSphereSphere(s0, d, t0, mid, s1, out t))
{
return(true);
}
// Recursively test second half of interval
return(TestMovingSphereSphere(s0, d, mid, t1, s1, out t));
}
public static LVector3 Transform(LVector3 point, LVector3 forward, LVector3 trans, LVector3 scale)
{
LVector3 up = LVector3.up;
LVector3 vInt = Cross(LVector3.up, forward);
return(LMath.Transform(ref point, ref vInt, ref up, ref forward, ref trans, ref scale));
}
public void setRightPlane(LVector3 pivot, LVector3 rightEdgeVertex)
{
rightPlane.set(pivot, pivot.Add(LVector3.up), rightEdgeVertex); // 高度
rightPlane.normal = -rightPlane.normal; // 平面方向取反
rightPlane.d = -rightPlane.d;
rightPortal = (rightEdgeVertex);
}
public TriangleEdge(Triangle fromNode, Triangle toNode, LVector3 rightVertex, LVector3 leftVertex)
{
this.fromNode = fromNode;
this.toNode = toNode;
this.rightVertex = rightVertex;
this.leftVertex = leftVertex;
}
public STriangle(LVector3 a, LVector3 b, LVector3 c)
{
this.a = a;
this.b = b;
this.c = c;
this.n = Cross(b - a, c - a).normalized;
this.d = Dot(n, a);
this.min = a;
this.max = a;
for (int i = 0; i < 3; i++)
{
LFloat minv = a[i];
LFloat maxv = a[i];
var _b = b[i];
var _c = c[i];
if (_b < minv)
{
minv = _b;
}
if (_c < minv)
{
minv = _c;
}
if (_b > maxv)
{
maxv = _b;
}
if (_c > maxv)
{
maxv = _c;
}
min[i] = minv;
max[i] = maxv;
}
}
// Intersects ray r = p + td, |d| = 1, with sphere s and, if intersecting,
// returns t value of intersection and intersection point q
public static bool IntersectRaySphere(LVector3 p, LVector3 d, Sphere s, out LFloat t, out LVector3 q)
{
LVector3 m = p - s.c;
LFloat b = Dot(m, d);
LFloat c = Dot(m, m) - s.r * s.r;
t = LFloat.zero;
q = p;
// Exit if r’s origin outside s (c > 0)and r pointing away from s (b > 0)
if (c > LFloat.zero && b > LFloat.zero)
{
return(false);
}
LFloat discr = b * b - c;
// A negative discriminant corresponds to ray missing sphere
if (discr < LFloat.zero)
{
return(false);
}
// Ray now found to intersect sphere, compute smallest t value of intersection
t = -b - Sqrt(discr);
// If t is negative, ray started inside sphere so clamp t to zero
if (t < LFloat.zero)
{
t = LFloat.zero;
}
q = p + t * d;
return(true);
}
public void ShowBlockInventory(BlocksPlayer playerUsing, LVector3 block)
{
Inventory blockInventory;
BlockOrItem customBlock;
if (blocksWorld.world.BlockHasInventory(block, out blockInventory) && blocksWorld.blocksPack.customBlocks.ContainsKey(block.BlockV, out customBlock))
{
ShowPlayerInventory(playerUsing);
blockShowing = block;
blockShowingInventory = blockInventory;
blocksWorld.otherObjectInventoryGui.displaying = true;
blocksWorld.otherObjectInventoryGui.playerUsing = playerUsing;
blocksWorld.otherObjectInventoryGui.inventory = blockInventory;
blocksWorld.otherObjectInventoryGui.inventory = blockInventory;
blocksWorld.otherObjectInventoryGui.screenOffset = new Vector2(0, 300);
blocksWorld.otherObjectInventoryGui.customBlockOwner = customBlock;
blocksWorld.otherObjectInventoryGui.customBlockOwnerPosition = block;
blocksWorld.otherObjectInventoryGui.numRows = customBlock.NumInventoryRows();
showingInventory = true;
showingBlockInventory = true;
}
else
{
HidePlayerInventory();
blockShowing = LVector3.Invalid;
blockShowingInventory = null;
showingBlockInventory = false;
showingInventory = false;
}
}
public static bool IntersectRayOBB(LVector3 o, LVector3 d, OBB obb, out LFloat tmin, out LVector3 p)
{
var fo = o - obb.c;
var fd = obb.u.WorldToLocal(d);
return(Utils.IntersectRayAABB(fo, fd, obb.ToAABB(), out tmin, out p));
}
public static LVector3 Lerp(LVector3 a, LVector3 b, LFloat f)
{
return(new LVector3(true,
(int)(((long)(b._x - a._x) * f._val) / LFloat.Precision) + a._x,
(int)(((long)(b._y - a._y) * f._val) / LFloat.Precision) + a._y,
(int)(((long)(b._z - a._z) * f._val) / LFloat.Precision) + a._z));
}
public override void ApplyPositionCorrection(float deltaTime)
{
if ((objA.linearVelocity - objB.linearVelocity).sqrMagnitude <= 0)
{
LVector2 ca = objA.GetWorldCenterOfMass();
LVector2 ra = anchorA - ca;
LVector2 cb = objB.GetWorldCenterOfMass();
LVector2 rb = anchorB - cb;
LFloat pa = objA.mass / (objA.mass + objB.mass);
LVector3 dva = -(LVector3)normal * penetration * pa * ERP;
LVector3 dvb = (LVector3)normal * penetration * (1 - pa) * ERP;
LFloat dwa = LVector3.Cross(ra, dva * objA.mass).z *objA.inertiaInverse *deltaTime;
LFloat dwb = LVector3.Cross(rb, dvb * objB.mass).z *objB.inertiaInverse *deltaTime;
if (objA.rigidBody != null && !objA.rigidBody.isFixed)
{
objA.transform.position += LParser.Parse(dva);
objA.transform.eulerAngles += new Vector3(0, 0, (dwa * Mathf.Rad2Deg).ToFloat());
}
if (objB.rigidBody != null && !objB.rigidBody.isFixed)
{
objB.transform.position += LParser.Parse(dvb);
objB.transform.eulerAngles += new Vector3(0, 0, (dwb * Mathf.Rad2Deg).ToFloat());
}
}
}
public static LVector3 set(this LVector3 vec, LFloat x, LFloat y, LFloat z)
{
vec.x = x;
vec.y = y;
vec.z = z;
return(vec);
}
public static LVector3 mulAdd(this LVector3 _this, LVector3 vec, LFloat scalar)
{
_this.x += vec.x * scalar;
_this.y += vec.y * scalar;
_this.z += vec.z * scalar;
return(_this);
}
public static bool TestAABB(LVector3 min0, LVector3 max0, LVector3 min1, LVector3 max1,
ref CollisionResult result,
bool twod = true)
{
_distances[0] = max1[0] - min0[0];
_distances[1] = max0[0] - min1[0];
_distances[2] = max1[2] - min0[2];
_distances[3] = max0[2] - min1[2];
var iter = 4;
// test y if 3d
if (!twod)
{
_distances[4] = max1[1] - min0[1];
_distances[5] = max0[1] - min1[1];
iter = 6;
}
for (int i = 0; i < iter; i++)
{
if (_distances[i] < LFloat.zero)
{
return(false);
}
if ((i == 0) || _distances[i] < result.Penetration)
{
result.Penetration = _distances[i];
//result.Normal = _aabbNormals[i];
}
}
return(true);
}
public static int3 floor(LVector3 vec)
{
return(new int3(
LMath.FloorToInt(vec.x),
LMath.FloorToInt(vec.y),
LMath.FloorToInt(vec.z)
));
}
public static LVector3 Cross(LVector3 lhs, LVector3 rhs)
{
return(new LVector3(true,
((long)lhs._y * rhs._z - (long)lhs._z * rhs._y) / LFloat.Precision,
((long)lhs._z * rhs._x - (long)lhs._x * rhs._z) / LFloat.Precision,
((long)lhs._x * rhs._y - (long)lhs._y * rhs._x) / LFloat.Precision
));
}
public static LFloat Dot(LVector3 lhs, LVector3 rhs)
{
var val = ((long)lhs._x) * rhs._x + ((long)lhs._y) * rhs._y + ((long)lhs._z) * rhs._z;
return(new LFloat(true, val / LFloat.Precision));
;
}
public static LVector3 Cross(LVector3 lhs, LVector3 rhs)
{
return(new LVector3(
lhs.y * rhs.z - lhs.z * rhs.y,
lhs.z * rhs.x - lhs.x * rhs.z,
lhs.x * rhs.y - lhs.y * rhs.x
));
}
public virtual void TakeDamage(int amount, LVector3 hitPoint)
{
if (isInvincible || isDead)
{
return;
}
OnTakeDamage(amount, hitPoint);
}
