public static VIntQuaternion FromToRotation(VInt3 from, VInt3 to) { VInt3 axis = VInt3.Cross(from, to); VFixedPoint angle = FMath.Trig.acos(VInt3.Dot(from, to)); return(AngleAxis(angle, axis)); }
static int rayTriSpecial(VInt3 orig, VInt3 dir, VInt3 vert0, VInt3 edge1, VInt3 edge2, ref VFixedPoint t, ref VFixedPoint u, ref VFixedPoint v) { VInt3 pvec = VInt3.Cross(dir, edge2); VFixedPoint det = VInt3.Dot(edge1, pvec); //triangle lies in plane of triangle if (det > -Globals.EPS && det < Globals.EPS) { return(0); } VFixedPoint oneOverDet = VFixedPoint.One / det; VInt3 tvec = orig - vert0; u = VInt3.Dot(tvec, pvec) * oneOverDet; VInt3 qvec = VInt3.Cross(tvec, edge1); v = VInt3.Dot(dir, qvec) * oneOverDet; if (u < VFixedPoint.Zero || u > VFixedPoint.One) { return(1); } if (v < VFixedPoint.Zero || u + v > VFixedPoint.One) { return(1); } t = VInt3.Dot(edge2, qvec) * oneOverDet; return(2); }
public void SetNewDirection(ref VInt3 dir) { bool flag = false; if (this.enabled) { if (VInt3.Dot(ref this.firstDir, ref dir) < 990268) { flag = true; } } else { flag = true; } if (flag) { this.adjDir = dir; this.firstAdjDir = VInt3.zero; this.curAdjDir = VInt3.zero; this.firstDir = VInt3.zero; this.enabled = false; } this.curDir = dir; }
static bool cullTriangle(VInt3[] triVerts, VInt3 dir, VFixedPoint radius, VFixedPoint t, VFixedPoint dpc0) { // PT: project triangle on axis VFixedPoint dp0 = VInt3.Dot(triVerts[0], dir); VFixedPoint dp1 = VInt3.Dot(triVerts[1], dir); VFixedPoint dp2 = VInt3.Dot(triVerts[2], dir); // PT: keep min value = earliest possible impact distance VFixedPoint dp = dp0; dp = FMath.Min(dp, dp1); dp = FMath.Min(dp, dp2); // PT: make sure we keep triangles that are about as close as best current distance radius += Globals.EPS; // PT: if earliest possible impact distance for this triangle is already larger than // sphere's current best known impact distance, we can skip the triangle if (dp > dpc0 + t + radius) { return(false); } // PT: if triangle is fully located before the sphere's initial position, skip it too VFixedPoint dpc1 = dpc0 - radius; if (dp0 < dpc1 && dp1 < dpc1 && dp2 < dpc1) { return(false); } return(true); }
// Returns true if sphere can be tested against triangle vertex, false if edge test should be performed // // Uses a conservative approach to work for "sliver triangles" (long & thin) as well. static bool edgeOrVertexTest(VInt3 planeIntersectionPoint, VInt3[] tri, int vertIntersectCandidate, int vert0, int vert1, ref int secondEdgeVert) { { VInt3 edge0 = tri[vertIntersectCandidate] - tri[vert0]; VFixedPoint edge0LengthSqr = edge0.sqrMagnitude; VInt3 diff = planeIntersectionPoint - tri[vert0]; if (VInt3.Dot(edge0, diff) < edge0LengthSqr) { secondEdgeVert = vert0; return(false); } } { VInt3 edge1 = tri[vertIntersectCandidate] - tri[vert1]; VFixedPoint edge1LengthSqr = edge1.sqrMagnitude; VInt3 diff = planeIntersectionPoint - tri[vert1]; if (VInt3.Dot(edge1, diff) < edge1LengthSqr) { secondEdgeVert = vert1; return(false); } } return(true); }
public bool isValid2(int i0, int i1, int i2, VInt3[] aBuf, VInt3[] bBuf, VFixedPoint lower, VFixedPoint upper) { VInt3 pa0 = aBuf[i0]; VInt3 pa1 = aBuf[i1]; VInt3 pa2 = aBuf[i2]; VInt3 pb0 = bBuf[i0]; VInt3 pb1 = bBuf[i1]; VInt3 pb2 = bBuf[i2]; VInt3 p0 = pa0 - pb0; VInt3 p1 = pa1 - pb1; VInt3 p2 = pa2 - pb2; VInt3 v1 = p1 - p0; VInt3 v2 = p2 - p0; VInt3 denormalizedNormal = VInt3.Cross(v1, v2); VInt3 planeNormal = denormalizedNormal.Normalize(); VFixedPoint planeDist = VInt3.Dot(planeNormal, p0); m_planeNormal = planeNormal; m_planeDist = planeDist; return(planeDist >= lower && upper >= planeDist); }
public static float Angle(VInt3 lhs, VInt3 rhs) { double num = (double)VInt3.Dot(lhs, rhs) / ((double)lhs.magnitude * (double)rhs.magnitude); num = ((num >= -1.0) ? ((num <= 1.0) ? num : 1.0) : -1.0); return((float)Math.Acos(num)); }
/* * A segment is defined by S(t) = mP0 * (1 - t) + mP1 * t, with 0 <= t <= 1 * Alternatively, a segment is S(t) = Origin + t * Direction for 0 <= t <= 1. * Direction is not necessarily unit length. The end points are Origin = mP0 and Origin + Direction = mP1. */ public static VFixedPoint distancePointSegmentSquared(VInt3 p0, VInt3 p1, VInt3 point, ref VFixedPoint param) { VInt3 Diff = point - p0; VInt3 Dir = p1 - p0; VFixedPoint ft = VInt3.Dot(Diff, Dir); if (ft <= VFixedPoint.Zero) { ft = VFixedPoint.Zero; } else { VFixedPoint sqrLen = Dir.sqrMagnitude; if (ft >= sqrLen) { ft = VFixedPoint.One; Diff -= Dir; } else { ft /= sqrLen; Diff -= Dir * ft; } } param = ft; return(Diff.sqrMagnitude); }
public VFixedPoint getPlaneDist(VInt3 p, VInt3[] aBuf, VInt3[] bBuf) { VInt3 pa0 = aBuf[m_indices[0]]; VInt3 pb0 = bBuf[m_indices[0]]; VInt3 p0 = pa0 - pb0; return(VInt3.Dot(m_planeNormal, p - p0)); }
public VInt3 Transform(VInt3 input) { VFixedPoint x = VInt3.Dot(new VInt3(matrix[0, 0], matrix[0, 1], matrix[0, 2]), input); VFixedPoint y = VInt3.Dot(new VInt3(matrix[1, 0], matrix[1, 1], matrix[1, 2]), input); VFixedPoint z = VInt3.Dot(new VInt3(matrix[2, 0], matrix[2, 1], matrix[2, 2]), input); return(new VInt3(x, y, z)); }
static VFixedPoint squareDistance(VInt3 p0, VInt3 dir, VFixedPoint t, VInt3 point) { VInt3 diff = point - p0; VFixedPoint fT = VInt3.Dot(diff, dir); fT = FMath.Min(FMath.Max(fT, VFixedPoint.Zero), t); diff -= dir * fT; return(diff.sqrMagnitude); }
// 0 p is inside abcd, 1 p is outside abcd public static int pointOutsideOfPlane(VInt3 p, VInt3 a, VInt3 b, VInt3 c, VInt3 d) { VInt3 normal = VInt3.Cross(b - a, c - a); VFixedPoint signp = VInt3.Dot(p - a, normal); // [AP AB AC] VFixedPoint signd = VInt3.Dot(d - a, normal); // [AD AB AC] VFixedPoint dp = signd * signd; return((signp * signd > Globals.EPS)? 1 : 0); }
public static int extrudeMesh(Triangle[] triangles, VInt3 extrudeDir, VInt3 dir, Triangle[] tris) { int num = 0; for (int i = 0; i < triangles.Length; i++) { Triangle currentTriangle = triangles[i]; VInt3 denormalizedNormal = currentTriangle.denormalizedNormal; bool culled = VInt3.Dot(denormalizedNormal, dir) > VFixedPoint.Zero; if (culled) { continue; } VInt3 p0 = currentTriangle.verts[0]; VInt3 p1 = currentTriangle.verts[1]; VInt3 p2 = currentTriangle.verts[2]; VInt3 p0b = p0 + extrudeDir; VInt3 p1b = p1 + extrudeDir; VInt3 p2b = p2 + extrudeDir; p0 -= extrudeDir; p1 -= extrudeDir; p2 -= extrudeDir; if (VInt3.Dot(denormalizedNormal, extrudeDir) >= VFixedPoint.Zero) { OUTPUT_TRI(tris, ref num, p0b, p1b, p2b); } else { OUTPUT_TRI(tris, ref num, p0, p1, p2); } { OUTPUT_TRI2(tris, ref num, p1, p1b, p2b, dir); OUTPUT_TRI2(tris, ref num, p1, p2b, p2, dir); } { OUTPUT_TRI2(tris, ref num, p0, p2, p2b, dir); OUTPUT_TRI2(tris, ref num, p0, p2b, p0b, dir); } { OUTPUT_TRI2(tris, ref num, p0b, p1b, p1, dir); OUTPUT_TRI2(tris, ref num, p0b, p1, p0, dir); } } return(num); }
public override void Process(Action _action, Track _track, int _localTime) { if (!this.actorTarget) { return; } int num = _localTime - this.lastTime; this.lastTime = _localTime; if (this.actorTarget.get_handle().ActorControl.curMoveCommand != null) { FrameCommand <MoveDirectionCommand> frameCommand = (FrameCommand <MoveDirectionCommand>) this.actorTarget.get_handle().ActorControl.curMoveCommand; VInt3 vInt = this.actorTarget.get_handle().forward; VInt3 right = VInt3.right; this.destDir = right.RotateY((int)frameCommand.cmdData.Degree); if (this.destDir != vInt) { this.bNeedRotate = true; this.curRotateSpd = this.rotateSpeed; int num2 = this.destDir.x * vInt.z - vInt.x * this.destDir.z; if (num2 == 0) { int num3 = VInt3.Dot(this.destDir, vInt); if (num3 >= 0) { return; } this.curRotateSpd = this.rotateSpeed; } else if (num2 < 0) { this.curRotateSpd = -this.rotateSpeed; } VFactor vFactor = VInt3.AngleInt(this.destDir, vInt); VFactor vFactor2 = VFactor.pi * (long)num * (long)this.curRotateSpd / 180L / 1000L; if (vFactor <= vFactor2) { vInt = vInt.RotateY(ref vFactor); this.bNeedRotate = false; } else { vInt = vInt.RotateY(ref vFactor2); } this.actorTarget.get_handle().MovementComponent.SetRotate(vInt, true); } } else { this.destDir = this.actorTarget.get_handle().forward; this.bNeedRotate = false; this.curRotateSpd = 0; } base.Process(_action, _track, _localTime); }
public static VIntQuaternion operator *(VIntQuaternion lhs, VIntQuaternion rhs) { VInt3 lvirtual = new VInt3(lhs.x, lhs.y, lhs.z); VFixedPoint lreal = lhs.w; VInt3 rvirtual = new VInt3(rhs.x, rhs.y, rhs.z); VFixedPoint rreal = rhs.w; VInt3 virtualPart = VInt3.Cross(lvirtual, rvirtual) + rvirtual * lreal + lvirtual * rreal; VFixedPoint realPart = lreal * rreal - VInt3.Dot(lvirtual, rvirtual); return(new VIntQuaternion(virtualPart.x, virtualPart.y, virtualPart.z, realPart)); }
protected void stepForwardAndStrafe(CollisionWorld collisionWorld, VInt3 walkMove) { VIntTransform start = VIntTransform.Identity, end = VIntTransform.Identity; targetPosition = currentPosition + walkMove; VFixedPoint fraction = VFixedPoint.One; int maxIter = 10; while (fraction > VFixedPoint.Create(0.01f) && maxIter-- > 0) { start.position = currentPosition; end.position = targetPosition; List <CastResult> results = new List <CastResult>(); me.setWorldTransform(start); collisionWorld.SweepTest(me, end.position, results); if (results.Count > 0) { VFixedPoint closestHitFraction = results[0].fraction; VInt3 hitNormalWorld = results[0].normal; for (int i = 1; i < results.Count; i++) { VFixedPoint afraction = results[i].fraction; if (afraction <= closestHitFraction) { closestHitFraction = afraction; hitNormalWorld = results[i].normal; } } fraction -= closestHitFraction; updateTargetPositionBasedOnCollision(hitNormalWorld); VInt3 currentDir = targetPosition - currentPosition; VFixedPoint distance2 = currentDir.sqrMagnitude; if (distance2 > Globals.EPS2) { currentDir = currentDir.Normalize(); if (VInt3.Dot(currentDir, normalizedDirection) <= VFixedPoint.Zero) { break; } } else { break; } } else { currentPosition = targetPosition; } } }
public VInt3 ClosestPoint(VInt3 targetPoint) { VInt3 vInt = targetPoint - this.worldPos; VInt3 lhs = this.worldPos; int num = this.worldExtends.x * 1000; int num2 = this.worldExtends.y * 1000; int num3 = this.worldExtends.z * 1000; lhs += IntMath.Divide(this.axis[0], (long)Mathf.Clamp(VInt3.Dot(ref vInt, ref this.axis[0]), -num, num), 1000000L); lhs += IntMath.Divide(this.axis[1], (long)Mathf.Clamp(VInt3.Dot(ref vInt, ref this.axis[1]), -num2, num2), 1000000L); return(lhs + IntMath.Divide(this.axis[2], (long)Mathf.Clamp(VInt3.Dot(ref vInt, ref this.axis[2]), -num3, num3), 1000000L)); }
public VInt3 ClosestPoint(VInt3 targetPoint) { VInt3 lhs = targetPoint - this.worldPos; VInt3 worldPos = this.worldPos; int max = this.worldExtends.x * 0x3e8; int num4 = this.worldExtends.y * 0x3e8; int num5 = this.worldExtends.z * 0x3e8; worldPos += IntMath.Divide(this.axis[0], (long)Mathf.Clamp(VInt3.Dot(ref lhs, ref this.axis[0]), -max, max), 0xf4240L); worldPos += IntMath.Divide(this.axis[1], (long)Mathf.Clamp(VInt3.Dot(ref lhs, ref this.axis[1]), -num4, num4), 0xf4240L); return(worldPos + IntMath.Divide(this.axis[2], (long)Mathf.Clamp(VInt3.Dot(ref lhs, ref this.axis[2]), -num5, num5), 0xf4240L)); }
public static float NearestPointFactor(VInt3 lineStart, VInt3 lineEnd, VInt3 point) { VInt3 rhs = lineEnd - lineStart; double sqrMagnitude = rhs.sqrMagnitude; double num3 = VInt3.Dot(point - lineStart, rhs); if (sqrMagnitude != 0.0) { num3 /= sqrMagnitude; } return((float)num3); }
/** Factor along the line which is closest to the point. * Returned value is in the range [0,1] if the point lies on the segment otherwise it just lies on the line. * The closest point can be calculated using (end-start)*factor + start */ public static long ClosestPointOnLineFactor(VInt3 lineStart, VInt3 lineEnd, VInt3 point) { var lineDirection = lineEnd - lineStart; long magn = lineDirection.sqrMagnitudeLong; long closestPoint = VInt3.Dot((point - lineStart), lineDirection); if (magn != 0) { closestPoint /= magn; } return(closestPoint); }
public static bool sphereSphereSweep(VFixedPoint ra, // radius of sphere A VInt3 A0, //from position of sphere A VInt3 A1, //to position of sphere A VFixedPoint rb, //radius of sphere B VInt3 B0, //position of sphere B ref VFixedPoint u0, //normalized time of first collision ref VFixedPoint u1, //normalized time of second collision ref VInt3 normal ) { VInt3 va = A1 - A0; VInt3 AB = B0 - A0; VInt3 vab = -va; VFixedPoint rab = ra + rb; VFixedPoint a = vab.sqrMagnitude; VFixedPoint b = VInt3.Dot(vab, AB) * 2; VFixedPoint c = AB.sqrMagnitude - rab * rab; //check if they're currently overlapping if (c <= VFixedPoint.Zero || a == VFixedPoint.Zero) { u0 = VFixedPoint.Zero; normal = A0 - B0; return(true); } if (quadraticFormula(a, b, c, ref u0, ref u1)) { if (u0 > u1) { VFixedPoint tmp = u0; u0 = u1; u1 = tmp; } if (u1 < VFixedPoint.Zero || u0 > VFixedPoint.One) { return(false); } normal = ((A0 + va * u0) - B0).Normalize(); return(true); } return(false); }
public static void transformAabb(VInt3 halfExtents, VFixedPoint margin, VIntTransform trans, out VInt3 aabbMinOut, out VInt3 aabbMaxOut) { VInt3 halfExtentsWithMargin = halfExtents; halfExtentsWithMargin.x += margin; halfExtentsWithMargin.y += margin; halfExtentsWithMargin.z += margin; VInt3 center = trans.position; VInt3[] basis = trans.getTransposeBasis(); VInt3 extent = new VInt3(VInt3.Dot(basis[0].Abs(), halfExtents), VInt3.Dot(basis[1].Abs(), halfExtents), VInt3.Dot(basis[2].Abs(), halfExtents)); aabbMinOut = center - extent; aabbMaxOut = center + extent; }
public override void getAabb(VIntTransform t, out VInt3 aabbMin, out VInt3 aabbMax) { VInt3 halfExtents = p0.Abs(); VInt3[] basis = t.getTransposeBasis(); VInt3 center = t.position; VInt3 extent = new VInt3(); extent.x = VInt3.Dot(halfExtents, basis[0].Abs()); extent.y = VInt3.Dot(halfExtents, basis[1].Abs()); extent.z = VInt3.Dot(halfExtents, basis[2].Abs()); aabbMin = center - extent; aabbMax = center + extent; }
/** Returns the closest point on the segment in the XZ plane. * The y coordinate of the result will be the same as the y coordinate of the \a point parameter. * * The segment is NOT treated as infinite. * \see ClosestPointOnSegment * \see ClosestPointOnLine */ public static VInt3 ClosestPointOnSegmentXZ(VInt3 lineStart, VInt3 lineEnd, VInt3 point) { lineStart.y = point.y; lineEnd.y = point.y; VInt3 fullDirection = lineEnd - lineStart; VInt3 fullDirection2 = fullDirection; fullDirection2.y = 0; int magn = fullDirection2.magnitude; VInt3 lineDirection = magn > float.Epsilon ? fullDirection2 / magn : VInt3.zero; int closestPoint = VInt3.Dot((point - lineStart), lineDirection); int a = (int)Clamp(closestPoint, 0, fullDirection2.magnitude); return(lineStart + (lineDirection * a)); }
static bool computeSphereTriangleImpactData(int index, VFixedPoint curT, VInt3 center, VInt3 unitDir, VInt3 bestTriNormal, Triangle[] triangles, ref VFixedPoint fraction, ref VInt3 hitNormal) { if (index < 0) { return(false); } if (VInt3.Dot(bestTriNormal, unitDir) > VFixedPoint.Zero) { bestTriNormal *= -1; } fraction = curT; hitNormal = bestTriNormal; return(true); }
protected void stepDown(CollisionWorld collisionWorld, VFixedPoint dt) { VIntTransform start = VIntTransform.Identity, end = VIntTransform.Identity; VFixedPoint additionalDownStep = wasOnGround ? stepHeight : VFixedPoint.Zero; VInt3 stepDrop = upAxisDirection[upAxis] * (currentStepOffset + additionalDownStep); VFixedPoint downVelocity = (additionalDownStep == VFixedPoint.Zero && verticalVelocity < VFixedPoint.Zero ? -verticalVelocity : VFixedPoint.Zero) * dt; VInt3 gravityDrop = upAxisDirection[upAxis] * downVelocity; targetPosition -= stepDrop; targetPosition -= gravityDrop; start.position = currentPosition; end.position = targetPosition; List <CastResult> results = new List <CastResult>(); me.setWorldTransform(start); collisionWorld.SweepTest(me, end.position, results); if (results.Count > 0) { VFixedPoint closestHitFraction = VFixedPoint.One; for (int i = 0; i < results.Count; i++) { VFixedPoint fraction = results[i].fraction; if (VInt3.Dot(results[i].normal, upAxisDirection[upAxis]) < maxSlopeCosine) { continue; } if (fraction < closestHitFraction) { closestHitFraction = fraction; } } // we dropped a fraction of the height -> hit floor currentPosition = currentPosition * (VFixedPoint.One - closestHitFraction) + targetPosition * closestHitFraction; verticalVelocity = VFixedPoint.Zero; verticalOffset = VFixedPoint.Zero; } else { // we dropped the full height currentPosition = targetPosition; } }
public override void getAabb(VIntTransform t, out VInt3 aabbMin, out VInt3 aabbMax) { VInt3 halfExtents = VInt3.zero; halfExtents[upAxis] = getHalfHeight(); VInt3[] basis = t.getTransposeBasis(); VInt3 center = t.position; VInt3 extent = new VInt3(); extent.x = VInt3.Dot(halfExtents, basis[0].Abs()); extent.y = VInt3.Dot(halfExtents, basis[1].Abs()); extent.z = VInt3.Dot(halfExtents, basis[2].Abs()); aabbMin = center - extent - VInt3.one * getRadius(); aabbMax = center + extent + VInt3.one * getRadius(); }
static void OUTPUT_TRI2(Triangle[] triangles, ref int index, VInt3 p0, VInt3 p1, VInt3 p2, VInt3 d) { Triangle t = triangles[index]; t.verts[0] = p0; t.verts[1] = p1; t.verts[2] = p2; VInt3 denormalizedNormal = triangles[index].denormalizedNormal; if (VInt3.Dot(denormalizedNormal, d) > VFixedPoint.Zero) { VInt3 tmp = t.verts[1]; t.verts[1] = t.verts[2]; t.verts[2] = tmp; } index++; }
static VFixedPoint calculatePlaneDist(int i0, int i1, int i2, VInt3[] aBuf, VInt3[] bBuf) { VInt3 pa0 = aBuf[i0]; VInt3 pa1 = aBuf[i1]; VInt3 pa2 = aBuf[i2]; VInt3 pb0 = bBuf[i0]; VInt3 pb1 = bBuf[i1]; VInt3 pb2 = bBuf[i2]; VInt3 p0 = pa0 - pb0; VInt3 p1 = pa1 - pb1; VInt3 p2 = pa2 - pb2; VInt3 v1 = p1 - p0; VInt3 v2 = p2 - p0; VInt3 planeNormal = VInt3.Cross(v1, v2).Normalize(); return(VInt3.Dot(planeNormal, p0)); }
/** Adds obstacles for a grid graph */ void AddGraphObstacles(Pathfinding.RVO.Simulator sim, GridGraph grid) { //GG //bool reverse = Vector3.Dot(grid.transform.TransformVector(Vector3.up), sim.movementPlane == MovementPlane.XY ? Vector3.back : Vector3.up) > 0; bool reverse = VInt3.Dot(grid.transform.TransformVector(VInt3.up), sim.movementPlane == MovementPlane.XY ? VInt3.back : VInt3.up) > 0; GraphUtilities.GetContours(grid, vertices => { // Check if the contour is traced in the wrong direction from the one we want it in. // If we did not do this then instead of the obstacles keeping the agents OUT of the walls // they would keep them INSIDE the walls. if (reverse) { System.Array.Reverse(vertices); } //GG //obstacles.Add(sim.AddObstacle(vertices, wallHeight, true)); obstacles.Add(sim.AddObstacle(vertices, (int)wallHeight, true)); //GG //}, wallHeight*0.4f); }, (int)wallHeight * new VFactor(2, 5)); }