/// <summary> /// Gets the closest point on the segment to the given point. /// </summary> /// <param name="p">The point with which to calculate the nearest segment point.</param> /// <param name="scalar">Returns a value between 0 and 1 indicating the location on the segment nearest the point.</param> /// <param name="output">Returns the closest point on the segment.</param> public void ClosestPointTo(ref TSVector p, out FP scalar, out TSVector output) { TSVector u, v; TSVector.Subtract(ref p, ref P1, out v); TSVector.Subtract(ref P2, ref P1, out u); scalar = TSVector.Dot(ref u, ref v); scalar /= u.sqrMagnitude; if (scalar <= FP.Zero) { output = P1; } else if (scalar >= FP.One) { output = P2; } else { TSVector.Multiply(ref u, scalar, out output); TSVector.Add(ref P1, ref output, out output); } }
public void DebugDraw(IDebugDrawer drawer) { TSVector pos1, pos2, pos3; for (int i = 0; i < hullPoints.Count; i += 3) { pos1 = hullPoints[i + 0]; pos2 = hullPoints[i + 1]; pos3 = hullPoints[i + 2]; TSVector.Transform(pos1, orientation, out pos1); TSVector.Add(pos1, position, out pos1); TSVector.Transform(pos2, orientation, out pos2); TSVector.Add(pos2, position, out pos2); TSVector.Transform(pos3, orientation, out pos3); TSVector.Add(pos3, position, out pos3); drawer.DrawTriangle(pos1, pos2, pos3); } }
public LimitedHingeJoint(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis, FP minLimit, FP maxLimit) : base(world, body1, body2, position, hingeAxis) { TSVector perpDir = TSVector.up; if (TSVector.Dot(perpDir, hingeAxis) > 0.1f) { perpDir = TSVector.right; } TSVector sideAxis = TSVector.Cross(hingeAxis, perpDir); perpDir = TSVector.Cross(sideAxis, hingeAxis); perpDir.Normalize(); FP len = 15; TSVector hingeRelAnchorPos0 = perpDir * len; FP angleToMiddle = FP.Half * (minLimit - maxLimit); TSMatrix outMatrix; TSMatrix.CreateFromAxisAngle(ref hingeAxis, -angleToMiddle * FP.Deg2Rad, out outMatrix); TSVector hingeRelAnchorPos1 = TSVector.Transform(hingeRelAnchorPos0, outMatrix); FP hingeHalfAngle = FP.Half * (minLimit + maxLimit); FP allowedDistance = len * 2 * FP.Sin(hingeHalfAngle * FP.Half * FP.Deg2Rad); TSVector hingePos = body1.TSPosition; TSVector relPos0c = hingePos + hingeRelAnchorPos0; TSVector relPos1c = hingePos + hingeRelAnchorPos1; distance = new PointPointDistance((RigidBody)body1, (RigidBody)body2, relPos0c, relPos1c); distance.Distance = allowedDistance; distance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance; StateTracker.AddTracking(distance); }
/// <summary> /// Iteratively solve this constraint. /// </summary> public override void Iterate() { // calculate opposite force needed compare to velocity with respect to jacobian direction vel normalized. //FP body1VelNeeded = body1.linearVelocity * jacobian[0] * -effectiveMass; //FP body1AngularVelNeeded = body1.angularVelocity * jacobian[1] * -effectiveMass * j; //FP body2VelNeeded = body2.linearVelocity * jacobian[2] * -effectiveMass; //FP body2AngularVelNeeded = body2.angularVelocity * jacobian[3] * -effectiveMass; // total resisit value needed. FP jv = body1.linearVelocity * jacobian[0] + body2.linearVelocity * jacobian[2]; //UnityEngine.Debug.Log("effective mass " + effectiveMass); //FP softnessScalar = accumulatedImpulse * softnessOverDt; FP lambda = jv + bias; // accumulatedImpulse += -effectiveMass * lambda; if (!body1.isStatic) { //body1.linearVelocity = TSVector.zero; body1.linearVelocity = (body1.linearVelocity - body2.linearVelocity) * effectiveMass; //body1.angularVelocity = TSVector.zero; body1.angularVelocity += TSVector.Transform((lambda) * -effectiveMass * jacobian[1], body1.invInertiaWorld); } if (!body2.isStatic) { //body2.linearVelocity += body2.inverseMass * -effectiveMass * (lambda) * jacobian[2]; //body2.linearVelocity = TSVector.zero; body2.linearVelocity = (body2.linearVelocity - body1.linearVelocity) * effectiveMass; //body2.angularVelocity = TSVector.zero; body2.angularVelocity += TSVector.Transform((lambda) * -effectiveMass * jacobian[3], body2.invInertiaWorld); } }
public override void SetDesiredPosOrVelocity(FP deltaTime, IsPosWalkable w = null, bool bSkipBoids = false) { bool bValidTargetPos = (_targetPos != TSVector.MinValue && _targetPos != TSVector.MaxValue); if (!bValidTargetPos) { return; } TSVector vec = _behaviour.position;// FP fDtSecond = deltaTime; //List<IAgentBehaviour> agents = _behaviour.neighbours; int preBoidsType = _activeBoids; bool bUsePreBoidsForce = bSkipBoids && _preBoidsForce != TSVector.zero; _activeBoids = !bUsePreBoidsForce ? _activeBoids : (byte)EBoidsActiveType.none; // bool bStatic = false; //TSVector a = ApplyForce(deltaTime,_targetPos-_behaviour.position , _behaviour.velocity, vec, null // , agents, bUsePreBoidsForce, false); //TSVector.zero;// //TSVector deltaV = a * fDtSecond; //FP maxSpeedSqr = _behaviour.maxSpeed * _behaviour.maxSpeed; //_behaviour.preVelocity = (_behaviour.velocity + deltaV);//.normalized*_behaviour.maxSpeed; //if (_behaviour.preVelocity.sqrMagnitude > _behaviour.maxSpeed * _behaviour.maxSpeed)//|| TSVector.Dot(_behaviour.velocity, a) > 0 //{ // _behaviour.preVelocity = CustomMath.Normalize(_behaviour.preVelocity) * _behaviour.maxSpeed; //} _behaviour.preVelocity = (_targetPos - _behaviour.position).normalized * _behaviour.maxSpeed; #if UNITY_EDITOR && !MULTI_THREAD if (PathFindingManager.DEBUG) { if (FP.Abs(_behaviour.preVelocity.x) > _behaviour.maxSpeed * 2 || FP.Abs(_behaviour.preVelocity.z) > _behaviour.maxSpeed * 2) { UnityEngine.Debug.LogError("velocity error!"); } } #endif _activeBoids = preBoidsType; }
/// <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); } TSVector sum = points[0]; TSVector.Add(sum, points[1], out sum); TSVector.Add(sum, points[2], out sum); TSVector.Multiply(sum, FP.One / (3 * FP.One), out sum); geomCen = sum; TSVector.Subtract(points[1], points[0], out sum); TSVector.Subtract(points[2], points[0], out normal); TSVector.Cross(sum, normal, out normal); }
private void UpdateArbiterContacts(Arbiter arbiter) { if (arbiter.contactList.Count == 0) { lock (removedArbiterStack) { removedArbiterStack.Push(arbiter); } return; } for (int i = arbiter.contactList.Count - 1; i >= 0; i--) { Contact c = arbiter.contactList[i]; c.UpdatePosition(); if (c.penetration < -contactSettings.breakThreshold) { Contact.Pool.GiveBack(c); arbiter.contactList.RemoveAt(i); continue; } else { TSVector diff; TSVector.Subtract(ref c.p1, ref c.p2, out diff); FP distance = TSVector.Dot(ref diff, ref c.normal); diff = diff - distance * c.normal; distance = diff.sqrMagnitude; // hack (multiplication by factor 100) in the // following line. if (distance > contactSettings.breakThreshold * contactSettings.breakThreshold * 100) { Contact.Pool.GiveBack(c); arbiter.contactList.RemoveAt(i); continue; } } } }
/// <summary> /// Gets the squared distance between the given point and the segment. /// </summary> /// <param name="p">The point with which to calculate the distance.</param> /// <returns>Returns the squared distance.</returns> public FP DistanceSquaredTo(ref TSVector p) { TSVector pq, pp, qp; FP e, f; TSVector.Subtract(ref P2, ref P1, out pq); TSVector.Subtract(ref p, ref P1, out pp); TSVector.Subtract(ref p, ref P2, out qp); e = TSVector.Dot(ref pp, ref pq); if (e <= FP.Zero) { return(pp.sqrMagnitude); } f = TSVector.Dot(ref pq, ref pq); if (e >= f) { return(qp.sqrMagnitude); } return(pp.sqrMagnitude - e * e / f); }
/// <summary> /// SupportMapping. Finds the point in the shape furthest away from the given direction. /// Imagine a plane with a normal in the search direction. Now move the plane along the normal /// until the plane does not intersect the shape. The last intersection point is the result. /// </summary> /// <param name="direction">The direction.</param> /// <param name="result">The result.</param> public override void SupportMapping(ref TSVector direction, out TSVector result) { FP r = FP.Sqrt(direction.x * direction.x + direction.z * direction.z); if (FP.Abs(direction.y) > FP.Zero) { TSVector dir; TSVector.Normalize(direction, out dir); TSVector.Multiply(dir, radius, out result); result.y += FP.Sign(direction.y) * FP.Half * length; } else if (r > FP.Zero) { result.x = direction.x / r * radius; result.y = FP.Zero; result.z = direction.z / r * radius; } else { result.x = FP.Zero; result.y = FP.Zero; result.z = FP.Zero; } }
public bool HasNeighboursBetween2Point(int excludeProxyId, TSVector pos1, TSVector pos2, Stack <int> queryStack) { #if USING_DYNAMIC_TREE TSVector2 vec1 = CustomMath.TSVecToVec2(pos1); TSVector2 vec2 = CustomMath.TSVecToVec2(pos2); if (_dynamicTree != null) { RayCastInput rc = new RayCastInput(); rc.p1 = vec1; rc.p2 = vec2; rc.maxFraction = 1;// GridMap.nodeSize * FP.EN1 * 5;/// 20*nodeSize; int proxyId = _dynamicTree.RayCast(null, ref rc, queryStack, true, excludeProxyId); if (proxyId >= 0) { // PathFindingAgentBehaviour blockedAgent = _dynamicTree.GetUserData(proxyId); return(true); } } return(false); #else return(false); #endif }
internal FlowField UpdateAgent(IAgentBehaviour agent, TSVector preDest, TSVector dest, GridMap map) { if (preDest != dest) { FlowField ff; bool hasPre = preDest != TSVector.MinValue; int idx = map.GetGridNodeId(preDest); if (hasPre && _dicFlowFields.TryGetValue(idx, out ff)) { ff.RemoveAgent(agent); } idx = map.GetGridNodeId(dest); if (!_dicFlowFields.TryGetValue(idx, out ff)) { ff = new FlowField(dest, map); _dicFlowFields[idx] = ff; } ff.AddAgent(agent); return(ff); } return(null); }
/// <summary> /// SupportMapping. Finds the point in the shape furthest away from the given direction. /// Imagine a plane with a normal in the search direction. Now move the plane along the normal /// until the plane does not intersect the shape. The last intersection point is the result. /// </summary> /// <param name="direction">The direction.</param> /// <param name="result">The result.</param> public override void SupportMapping(ref TSVector direction, out TSVector result) { FP sigma = FP.Sqrt(direction.x * direction.x + direction.z * direction.z); if (direction.y > direction.magnitude * sina) { result.x = FP.Zero; result.y = ((2 * FP.One) / (3)) * height; result.z = FP.Zero; } else if (sigma > FP.Zero) { result.x = radius * direction.x / sigma; result.y = -(FP.One / 3) * height; result.z = radius * direction.z / sigma; } else { result.x = FP.Zero; result.y = -(FP.One / 3) * height; result.z = FP.Zero; } }
internal GameObject Pop(TSVector position, TSQuaternion rotation) { GameObject returnobj = null; if (mGameList.Count > 0) { returnobj = mGameList[mGameList.Count - 1]; mGameList.RemoveAt(mGameList.Count - 1); } else { if (mAssetObj == null) { mAssetObj = _AssetManager.GetGameObject(mPath); mAssetObj.transform.SetParent(mRootTrans); mAssetObj.SetActive(false); } returnobj = TrueSyncManager.SyncedInstantiate(mAssetObj, position, rotation); } returnobj.transform.SetParent(mRootTrans); returnobj.SetActive(true); return(returnobj); }
private void AddPressureForces(FP timeStep) { if (pressure == FP.Zero || volume == FP.Zero) { return; } FP invVolume = FP.One / volume; foreach (Triangle t in triangles) { TSVector v1 = points[t.indices.I0].position; TSVector v2 = points[t.indices.I1].position; TSVector v3 = points[t.indices.I2].position; TSVector cross = (v3 - v1) % (v2 - v1); //TSVector center = (v1 + v2 + v3) * (FP.One / (3 * FP.One)); points[t.indices.I0].AddForce(invVolume * cross * pressure); points[t.indices.I1].AddForce(invVolume * cross * pressure); points[t.indices.I2].AddForce(invVolume * cross * pressure); } }
private void IntegrateForces() { for (int index = 0, length = rigidBodies.Count; index < length; index++) { RigidBody body = rigidBodies[index]; if (!body.isStatic && body.IsActive) { TSVector temp; TSVector.Multiply(ref body.force, body.inverseMass * timestep, out temp); TSVector.Add(ref temp, ref body.linearVelocity, out body.linearVelocity); if (!(body.isParticle)) { TSVector.Multiply(ref body.torque, timestep, out temp); TSVector.Transform(ref temp, ref body.invInertiaWorld, out temp); TSVector.Add(ref temp, ref body.angularVelocity, out body.angularVelocity); //clamp angular velocity to 0.5pi . FP maxAngVel = TSMath.PiOver2; FP angvel = body.angularVelocity.magnitude; if (angvel * timestep > maxAngVel) { body.angularVelocity *= (maxAngVel / timestep) / angvel; } } if (body.affectedByGravity) { TSVector.Multiply(ref gravity, timestep, out temp); TSVector.Add(ref body.linearVelocity, ref temp, out body.linearVelocity); } } body.force.MakeZero(); body.torque.MakeZero(); } }
/// <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); TSVector sum = vecs[0]; TSVector.Add(ref sum, ref vecs[1], out sum); TSVector.Add(ref sum, ref vecs[2], out sum); TSVector.Multiply(ref sum, FP.One / (3 * FP.One), out sum); geomCen = sum; TSVector.Subtract(ref vecs[1], ref vecs[0], out sum); TSVector.Subtract(ref vecs[2], ref vecs[0], out normal); TSVector.Cross(ref sum, ref normal, out normal); normal.Normalize(); if (flipNormal) { normal.Negate(); } }
/// <summary> /// Creates a matrix which rotates around the given axis by the given angle. /// </summary> /// <param name="axis">The axis.</param> /// <param name="angle">The angle.</param> /// <param name="result">The resulting rotation matrix</param> #region public static void CreateFromAxisAngle(ref JVector axis, FP angle, out JMatrix result) public static void CreateFromAxisAngle(ref TSVector axis, FP angle, out TSMatrix result) { FP x = axis.x; FP y = axis.y; FP z = axis.z; FP num2 = FP.Sin(angle); FP num = FP.Cos(angle); FP num11 = x * x; FP num10 = y * y; FP num9 = z * z; FP num8 = x * y; FP num7 = x * z; FP num6 = y * z; result.M11 = num11 + (num * (FP.One - num11)); result.M12 = (num8 - (num * num8)) + (num2 * z); result.M13 = (num7 - (num * num7)) - (num2 * y); result.M21 = (num8 - (num * num8)) - (num2 * z); result.M22 = num10 + (num * (FP.One - num10)); result.M23 = (num6 - (num * num6)) + (num2 * x); result.M31 = (num7 - (num * num7)) + (num2 * y); result.M32 = (num6 - (num * num6)) - (num2 * x); result.M33 = num9 + (num * (FP.One - num9)); }
/// <summary> /// Transforms the bounding box into the space given by orientation and position. /// </summary> /// <param name="position"></param> /// <param name="orientation"></param> /// <param name="result"></param> internal void InverseTransform(ref TSVector position, ref TSMatrix orientation) { TSVector.Subtract(ref max, ref position, out max); TSVector.Subtract(ref min, ref position, out min); TSVector center; TSVector.Add(ref max, ref min, out center); center.x *= FP.Half; center.y *= FP.Half; center.z *= FP.Half; TSVector halfExtents; TSVector.Subtract(ref max, ref min, out halfExtents); halfExtents.x *= FP.Half; halfExtents.y *= FP.Half; halfExtents.z *= FP.Half; TSVector.TransposedTransform(ref center, ref orientation, out center); TSMatrix abs; TSMath.Absolute(ref orientation, out abs); TSVector.TransposedTransform(ref halfExtents, ref abs, out halfExtents); TSVector.Add(ref center, ref halfExtents, out max); TSVector.Subtract(ref center, ref halfExtents, out min); }
// public void IniFlowField(int w, int h, TSVector destination)//destination from { #if USING_ASTAR_PROJECT && UNITY_EDITOR _gridWidth = Pathfinding.GridNode.GetGridGraph(0).width; _gridHeight = Pathfinding.GridNode.GetGridGraph(0).depth; #else _gridWidth = w; _gridHeight = h; #endif _destination = destination; if (_gridDijkstraCost == null) { _gridDijkstraCost = new FP[_gridWidth, _gridHeight]; } if (_flowField == null) { _flowField = new TSVector[_gridWidth, _gridHeight]; } if (_gridLOS == null) { _gridLOS = new bool[_gridWidth, _gridHeight]; } UpdateFlowField(); }
public void ApplyImpulse(TSVector impulse) { #region INLINE - HighFrequency //JVector temp; if (!treatBody1AsStatic) { body1.linearVelocity.x -= (impulse.x * body1.inverseMass); body1.linearVelocity.y -= (impulse.y * body1.inverseMass); body1.linearVelocity.z -= (impulse.z * body1.inverseMass); FP num0, num1, num2; num0 = relativePos1.y * impulse.z - relativePos1.z * impulse.y; num1 = relativePos1.z * impulse.x - relativePos1.x * impulse.z; num2 = relativePos1.x * impulse.y - relativePos1.y * impulse.x; FP num3 = (((num0 * body1.invInertiaWorld.M11) + (num1 * body1.invInertiaWorld.M21)) + (num2 * body1.invInertiaWorld.M31)); FP num4 = (((num0 * body1.invInertiaWorld.M12) + (num1 * body1.invInertiaWorld.M22)) + (num2 * body1.invInertiaWorld.M32)); FP num5 = (((num0 * body1.invInertiaWorld.M13) + (num1 * body1.invInertiaWorld.M23)) + (num2 * body1.invInertiaWorld.M33)); body1.angularVelocity.x -= num3; body1.angularVelocity.y -= num4; body1.angularVelocity.z -= num5; } if (!treatBody2AsStatic) { body2.linearVelocity.x += (impulse.x * body2.inverseMass); body2.linearVelocity.y += (impulse.y * body2.inverseMass); body2.linearVelocity.z += (impulse.z * body2.inverseMass); FP num0, num1, num2; num0 = relativePos2.y * impulse.z - relativePos2.z * impulse.y; num1 = relativePos2.z * impulse.x - relativePos2.x * impulse.z; num2 = relativePos2.x * impulse.y - relativePos2.y * impulse.x; FP num3 = (((num0 * body2.invInertiaWorld.M11) + (num1 * body2.invInertiaWorld.M21)) + (num2 * body2.invInertiaWorld.M31)); FP num4 = (((num0 * body2.invInertiaWorld.M12) + (num1 * body2.invInertiaWorld.M22)) + (num2 * body2.invInertiaWorld.M32)); FP num5 = (((num0 * body2.invInertiaWorld.M13) + (num1 * body2.invInertiaWorld.M23)) + (num2 * body2.invInertiaWorld.M33)); body2.angularVelocity.x += num3; body2.angularVelocity.y += num4; body2.angularVelocity.z += num5; } #endregion }
public bool ClosestPtPointTetrahedron(TSVector a, TSVector b, TSVector c, TSVector d, ref SubSimplexClosestResult finalResult) { tempResult.Reset(); // Start out assuming point inside all halfspaces, so closest to itself finalResult.closestPointOnSimplex = TSVector.zero; finalResult.usedVertices.Reset(); finalResult.usedVertices.UsedVertexA = true; finalResult.usedVertices.UsedVertexB = true; finalResult.usedVertices.UsedVertexC = true; finalResult.usedVertices.UsedVertexD = true; int pointOutsideABC = PointOutsideOfPlane(a, b, c, d); int pointOutsideACD = PointOutsideOfPlane(a, c, d, b); int pointOutsideADB = PointOutsideOfPlane(a, d, b, c); int pointOutsideBDC = PointOutsideOfPlane(b, d, c, a); if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) { finalResult.degenerate = true; return(false); } if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0) { return(false); } FP bestSqDist = FP.MaxValue; // If point outside face abc then compute closest point on abc if (pointOutsideABC != 0) { ClosestPtPointTriangle(a, b, c, ref tempResult); TSVector q = tempResult.closestPointOnSimplex; FP sqDist = q.sqrMagnitude; // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.closestPointOnSimplex = q; //convert result bitmask! finalResult.usedVertices.Reset(); finalResult.usedVertices.UsedVertexA = tempResult.usedVertices.UsedVertexA; finalResult.usedVertices.UsedVertexB = tempResult.usedVertices.UsedVertexB; finalResult.usedVertices.UsedVertexC = tempResult.usedVertices.UsedVertexC; finalResult.SetBarycentricCoordinates( tempResult.barycentricCoords[VertexA], tempResult.barycentricCoords[VertexB], tempResult.barycentricCoords[VertexC], FP.Zero); } } // Repeat test for face acd if (pointOutsideACD != 0) { ClosestPtPointTriangle(a, c, d, ref tempResult); TSVector q = tempResult.closestPointOnSimplex; //convert result bitmask! FP sqDist = q.sqrMagnitude; if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.closestPointOnSimplex = q; finalResult.usedVertices.Reset(); finalResult.usedVertices.UsedVertexA = tempResult.usedVertices.UsedVertexA; finalResult.usedVertices.UsedVertexC = tempResult.usedVertices.UsedVertexB; finalResult.usedVertices.UsedVertexD = tempResult.usedVertices.UsedVertexC; finalResult.SetBarycentricCoordinates( tempResult.barycentricCoords[VertexA], FP.Zero, tempResult.barycentricCoords[VertexB], tempResult.barycentricCoords[VertexC]); } } // Repeat test for face adb if (pointOutsideADB != 0) { ClosestPtPointTriangle(a, d, b, ref tempResult); TSVector q = tempResult.closestPointOnSimplex; //convert result bitmask! FP sqDist = q.sqrMagnitude; if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.closestPointOnSimplex = q; finalResult.usedVertices.Reset(); finalResult.usedVertices.UsedVertexA = tempResult.usedVertices.UsedVertexA; finalResult.usedVertices.UsedVertexD = tempResult.usedVertices.UsedVertexB; finalResult.usedVertices.UsedVertexB = tempResult.usedVertices.UsedVertexC; finalResult.SetBarycentricCoordinates( tempResult.barycentricCoords[VertexA], tempResult.barycentricCoords[VertexC], FP.Zero, tempResult.barycentricCoords[VertexB]); } } // Repeat test for face bdc if (pointOutsideBDC != 0) { ClosestPtPointTriangle(b, d, c, ref tempResult); TSVector q = tempResult.closestPointOnSimplex; //convert result bitmask! FP sqDist = q.sqrMagnitude; if (sqDist < bestSqDist) { bestSqDist = sqDist; finalResult.closestPointOnSimplex = q; finalResult.usedVertices.Reset(); finalResult.usedVertices.UsedVertexB = tempResult.usedVertices.UsedVertexA; finalResult.usedVertices.UsedVertexD = tempResult.usedVertices.UsedVertexB; finalResult.usedVertices.UsedVertexC = tempResult.usedVertices.UsedVertexC; finalResult.SetBarycentricCoordinates( FP.Zero, tempResult.barycentricCoords[VertexA], tempResult.barycentricCoords[VertexC], tempResult.barycentricCoords[VertexB]); } } //help! we ended up full ! if (finalResult.usedVertices.UsedVertexA && finalResult.usedVertices.UsedVertexB && finalResult.usedVertices.UsedVertexC && finalResult.usedVertices.UsedVertexD) { return(true); } return(true); }
public bool ClosestPtPointTriangle(TSVector a, TSVector b, TSVector c, ref SubSimplexClosestResult result) { result.usedVertices.Reset(); FP v, w; // Check if P in vertex region outside A TSVector ab = b - a; TSVector ac = c - a; FP d1 = TSVector.Dot(ab, a); FP d2 = TSVector.Dot(ac, a); if (d1 >= FP.Zero && d2 >= FP.Zero) { result.closestPointOnSimplex = a; result.usedVertices.UsedVertexA = true; result.SetBarycentricCoordinates(FP.One, FP.Zero, FP.Zero, FP.Zero); return(true); // a; // barycentric coordinates (1,0,0) } // Check if P in vertex region outside B FP d3 = TSVector.Dot(ab, b); FP d4 = TSVector.Dot(ac, b); if (d3 <= FP.Zero && d4 >= d3) { result.closestPointOnSimplex = b; result.usedVertices.UsedVertexB = true; result.SetBarycentricCoordinates(FP.Zero, FP.One, FP.Zero, FP.Zero); return(true); // b; // barycentric coordinates (0,1,0) } // Check if P in edge region of AB, if so return projection of P onto AB FP vc = d1 * d4 - d3 * d2; if (vc <= FP.Zero && d1 >= FP.Zero && d3 <= FP.Zero) { v = d1 / (d1 - d3); result.closestPointOnSimplex = a + v * ab; result.usedVertices.UsedVertexA = true; result.usedVertices.UsedVertexB = true; result.SetBarycentricCoordinates(FP.One - v, v, FP.Zero, FP.Zero); return(true); //return a + v * ab; // barycentric coordinates (1-v,v,0) } // Check if P in vertex region outside C FP d5 = TSVector.Dot(ab, c); FP d6 = TSVector.Dot(ac, c); if (d6 <= FP.Zero && d5 >= d6) { result.closestPointOnSimplex = c; result.usedVertices.UsedVertexC = true; result.SetBarycentricCoordinates(FP.Zero, FP.Zero, FP.One, FP.Zero); return(true);//c; // barycentric coordinates (0,0,1) } // Check if P in edge region of AC, if so return projection of P onto AC FP vb = d5 * d2 - d1 * d6; if (vb <= FP.Zero && d2 >= FP.Zero && d6 <= FP.Zero) { w = d2 / (d2 - d6); result.closestPointOnSimplex = a + w * ac; result.usedVertices.UsedVertexA = true; result.usedVertices.UsedVertexC = true; result.SetBarycentricCoordinates(FP.One - w, FP.Zero, w, FP.Zero); return(true); //return a + w * ac; // barycentric coordinates (1-w,0,w) } // Check if P in edge region of BC, if so return projection of P onto BC FP va = d3 * d6 - d5 * d4; FP d34 = d4 - d3; FP d65 = d5 - d6; if (va <= FP.Zero && d34 >= FP.Zero && d65 >= FP.Zero) { w = d34 / (d34 + d65); result.closestPointOnSimplex = b + w * (c - b); result.usedVertices.UsedVertexB = true; result.usedVertices.UsedVertexC = true; result.SetBarycentricCoordinates(FP.Zero, FP.One - w, w, FP.Zero); return(true); // return b + w * (c - b); // barycentric coordinates (0,1-w,w) } // P inside face region. Compute Q through its barycentric coordinates (u,v,w) FP denom = FP.One / (va + vb + vc); v = vb * denom; w = vc * denom; result.closestPointOnSimplex = a + ab * v + ac * w; result.usedVertices.UsedVertexA = true; result.usedVertices.UsedVertexB = true; result.usedVertices.UsedVertexC = true; result.SetBarycentricCoordinates(FP.One - v - w, v, w, FP.Zero); return(true); }
public void Do(int skillId, uint targetAgentId, AgentObjectType targetAgentType, TSVector position, TSVector forward) { Do(GetSkill(skillId), targetAgentId, targetAgentType, position, forward); }
public void ComputePoints(out TSVector p1, out TSVector p2) { UpdateClosestVectorAndPoints(); p1 = _cachedPA; p2 = _cachedPB; }
public void BackupClosest(out TSVector v) { v = _cachedV; }
/// <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) { FP dvx, dvy, dvz; dvx = (body2.angularVelocity.y * relativePos2.z) - (body2.angularVelocity.z * relativePos2.y) + body2.linearVelocity.x; dvy = (body2.angularVelocity.z * relativePos2.x) - (body2.angularVelocity.x * relativePos2.z) + body2.linearVelocity.y; dvz = (body2.angularVelocity.x * relativePos2.y) - (body2.angularVelocity.y * relativePos2.x) + body2.linearVelocity.z; dvx = dvx - (body1.angularVelocity.y * relativePos1.z) + (body1.angularVelocity.z * relativePos1.y) - body1.linearVelocity.x; dvy = dvy - (body1.angularVelocity.z * relativePos1.x) + (body1.angularVelocity.x * relativePos1.z) - body1.linearVelocity.y; dvz = dvz - (body1.angularVelocity.x * relativePos1.y) + (body1.angularVelocity.y * relativePos1.x) - body1.linearVelocity.z; FP kNormal = FP.Zero; TSVector rantra = TSVector.zero; if (!treatBody1AsStatic) { kNormal += body1.inverseMass; if (!body1IsMassPoint) { // JVector.Cross(ref relativePos1, ref normal, out rantra); rantra.x = (relativePos1.y * normal.z) - (relativePos1.z * normal.y); rantra.y = (relativePos1.z * normal.x) - (relativePos1.x * normal.z); rantra.z = (relativePos1.x * normal.y) - (relativePos1.y * normal.x); // JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra); FP num0 = ((rantra.x * body1.invInertiaWorld.M11) + (rantra.y * body1.invInertiaWorld.M21)) + (rantra.z * body1.invInertiaWorld.M31); FP num1 = ((rantra.x * body1.invInertiaWorld.M12) + (rantra.y * body1.invInertiaWorld.M22)) + (rantra.z * body1.invInertiaWorld.M32); FP num2 = ((rantra.x * body1.invInertiaWorld.M13) + (rantra.y * body1.invInertiaWorld.M23)) + (rantra.z * body1.invInertiaWorld.M33); rantra.x = num0; rantra.y = num1; rantra.z = num2; //JVector.Cross(ref rantra, ref relativePos1, out rantra); num0 = (rantra.y * relativePos1.z) - (rantra.z * relativePos1.y); num1 = (rantra.z * relativePos1.x) - (rantra.x * relativePos1.z); num2 = (rantra.x * relativePos1.y) - (rantra.y * relativePos1.x); rantra.x = num0; rantra.y = num1; rantra.z = num2; } } TSVector rbntrb = TSVector.zero; if (!treatBody2AsStatic) { kNormal += body2.inverseMass; if (!body2IsMassPoint) { // JVector.Cross(ref relativePos1, ref normal, out rantra); rbntrb.x = (relativePos2.y * normal.z) - (relativePos2.z * normal.y); rbntrb.y = (relativePos2.z * normal.x) - (relativePos2.x * normal.z); rbntrb.z = (relativePos2.x * normal.y) - (relativePos2.y * normal.x); // JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra); FP num0 = ((rbntrb.x * body2.invInertiaWorld.M11) + (rbntrb.y * body2.invInertiaWorld.M21)) + (rbntrb.z * body2.invInertiaWorld.M31); FP num1 = ((rbntrb.x * body2.invInertiaWorld.M12) + (rbntrb.y * body2.invInertiaWorld.M22)) + (rbntrb.z * body2.invInertiaWorld.M32); FP num2 = ((rbntrb.x * body2.invInertiaWorld.M13) + (rbntrb.y * body2.invInertiaWorld.M23)) + (rbntrb.z * body2.invInertiaWorld.M33); rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2; //JVector.Cross(ref rantra, ref relativePos1, out rantra); num0 = (rbntrb.y * relativePos2.z) - (rbntrb.z * relativePos2.y); num1 = (rbntrb.z * relativePos2.x) - (rbntrb.x * relativePos2.z); num2 = (rbntrb.x * relativePos2.y) - (rbntrb.y * relativePos2.x); rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2; } } if (!treatBody1AsStatic) { kNormal += rantra.x * normal.x + rantra.y * normal.y + rantra.z * normal.z; } if (!treatBody2AsStatic) { kNormal += rbntrb.x * normal.x + rbntrb.y * normal.y + rbntrb.z * normal.z; } massNormal = FP.One / kNormal; FP num = dvx * normal.x + dvy * normal.y + dvz * normal.z; tangent.x = dvx - normal.x * num; tangent.y = dvy - normal.y * num; tangent.z = dvz - normal.z * num; num = tangent.x * tangent.x + tangent.y * tangent.y + tangent.z * tangent.z; if (num != FP.Zero) { num = FP.Sqrt(num); tangent.x /= num; tangent.y /= num; tangent.z /= num; } FP kTangent = FP.Zero; if (treatBody1AsStatic) { rantra.MakeZero(); } else { kTangent += body1.inverseMass; if (!body1IsMassPoint) { // JVector.Cross(ref relativePos1, ref normal, out rantra); rantra.x = (relativePos1.y * tangent.z) - (relativePos1.z * tangent.y); rantra.y = (relativePos1.z * tangent.x) - (relativePos1.x * tangent.z); rantra.z = (relativePos1.x * tangent.y) - (relativePos1.y * tangent.x); // JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra); FP num0 = ((rantra.x * body1.invInertiaWorld.M11) + (rantra.y * body1.invInertiaWorld.M21)) + (rantra.z * body1.invInertiaWorld.M31); FP num1 = ((rantra.x * body1.invInertiaWorld.M12) + (rantra.y * body1.invInertiaWorld.M22)) + (rantra.z * body1.invInertiaWorld.M32); FP num2 = ((rantra.x * body1.invInertiaWorld.M13) + (rantra.y * body1.invInertiaWorld.M23)) + (rantra.z * body1.invInertiaWorld.M33); rantra.x = num0; rantra.y = num1; rantra.z = num2; //JVector.Cross(ref rantra, ref relativePos1, out rantra); num0 = (rantra.y * relativePos1.z) - (rantra.z * relativePos1.y); num1 = (rantra.z * relativePos1.x) - (rantra.x * relativePos1.z); num2 = (rantra.x * relativePos1.y) - (rantra.y * relativePos1.x); rantra.x = num0; rantra.y = num1; rantra.z = num2; } } if (treatBody2AsStatic) { rbntrb.MakeZero(); } else { kTangent += body2.inverseMass; if (!body2IsMassPoint) { // JVector.Cross(ref relativePos1, ref normal, out rantra); rbntrb.x = (relativePos2.y * tangent.z) - (relativePos2.z * tangent.y); rbntrb.y = (relativePos2.z * tangent.x) - (relativePos2.x * tangent.z); rbntrb.z = (relativePos2.x * tangent.y) - (relativePos2.y * tangent.x); // JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra); FP num0 = ((rbntrb.x * body2.invInertiaWorld.M11) + (rbntrb.y * body2.invInertiaWorld.M21)) + (rbntrb.z * body2.invInertiaWorld.M31); FP num1 = ((rbntrb.x * body2.invInertiaWorld.M12) + (rbntrb.y * body2.invInertiaWorld.M22)) + (rbntrb.z * body2.invInertiaWorld.M32); FP num2 = ((rbntrb.x * body2.invInertiaWorld.M13) + (rbntrb.y * body2.invInertiaWorld.M23)) + (rbntrb.z * body2.invInertiaWorld.M33); rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2; //JVector.Cross(ref rantra, ref relativePos1, out rantra); num0 = (rbntrb.y * relativePos2.z) - (rbntrb.z * relativePos2.y); num1 = (rbntrb.z * relativePos2.x) - (rbntrb.x * relativePos2.z); num2 = (rbntrb.x * relativePos2.y) - (rbntrb.y * relativePos2.x); rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2; } } if (!treatBody1AsStatic) { kTangent += TSVector.Dot(ref rantra, ref tangent); } if (!treatBody2AsStatic) { kTangent += TSVector.Dot(ref rbntrb, ref tangent); } massTangent = FP.One / kTangent; restitutionBias = lostSpeculativeBounce; speculativeVelocity = FP.Zero; FP relNormalVel = normal.x * dvx + normal.y * dvy + normal.z * dvz; //JVector.Dot(ref normal, ref dv); if (Penetration > settings.allowedPenetration) { restitutionBias = settings.bias * (FP.One / timestep) * TSMath.Max(FP.Zero, Penetration - settings.allowedPenetration); restitutionBias = TSMath.Clamp(restitutionBias, FP.Zero, settings.maximumBias); // body1IsMassPoint = body2IsMassPoint = false; } FP timeStepRatio = timestep / lastTimeStep; accumulatedNormalImpulse *= timeStepRatio; accumulatedTangentImpulse *= timeStepRatio; { // Static/Dynamic friction FP relTangentVel = -(tangent.x * dvx + tangent.y * dvy + tangent.z * dvz); FP tangentImpulse = massTangent * relTangentVel; FP maxTangentImpulse = -staticFriction * accumulatedNormalImpulse; if (tangentImpulse < maxTangentImpulse) { friction = dynamicFriction; } else { friction = staticFriction; } } TSVector 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. // modified by tiger, uncommmented. //if (relNormalVel < -FP.One && newContact) //{ // restitutionBias = TSMath.Max(-restitution * relNormalVel, restitutionBias); //} // added by seok //if (!newContact) if (!newContact || TSMath.Abs(relNormalVel *= restitution) < restitution) { relNormalVel = 0; } restitutionBias = TSMath.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.x = normal.x * accumulatedNormalImpulse + tangent.x * accumulatedTangentImpulse; impulse.y = normal.y * accumulatedNormalImpulse + tangent.y * accumulatedTangentImpulse; impulse.z = normal.z * accumulatedNormalImpulse + tangent.z * accumulatedTangentImpulse; if (!treatBody1AsStatic) { body1.linearVelocity.x -= (impulse.x * body1.inverseMass); body1.linearVelocity.y -= (impulse.y * body1.inverseMass); body1.linearVelocity.z -= (impulse.z * body1.inverseMass); if (!body1IsMassPoint) { FP num0, num1, num2; num0 = relativePos1.y * impulse.z - relativePos1.z * impulse.y; num1 = relativePos1.z * impulse.x - relativePos1.x * impulse.z; num2 = relativePos1.x * impulse.y - relativePos1.y * impulse.x; FP num3 = (((num0 * body1.invInertiaWorld.M11) + (num1 * body1.invInertiaWorld.M21)) + (num2 * body1.invInertiaWorld.M31)); FP num4 = (((num0 * body1.invInertiaWorld.M12) + (num1 * body1.invInertiaWorld.M22)) + (num2 * body1.invInertiaWorld.M32)); FP num5 = (((num0 * body1.invInertiaWorld.M13) + (num1 * body1.invInertiaWorld.M23)) + (num2 * body1.invInertiaWorld.M33)); body1.angularVelocity.x -= num3; body1.angularVelocity.y -= num4; body1.angularVelocity.z -= num5; } } if (!treatBody2AsStatic) { body2.linearVelocity.x += (impulse.x * body2.inverseMass); body2.linearVelocity.y += (impulse.y * body2.inverseMass); body2.linearVelocity.z += (impulse.z * body2.inverseMass); if (!body2IsMassPoint) { FP num0, num1, num2; num0 = relativePos2.y * impulse.z - relativePos2.z * impulse.y; num1 = relativePos2.z * impulse.x - relativePos2.x * impulse.z; num2 = relativePos2.x * impulse.y - relativePos2.y * impulse.x; FP num3 = (((num0 * body2.invInertiaWorld.M11) + (num1 * body2.invInertiaWorld.M21)) + (num2 * body2.invInertiaWorld.M31)); FP num4 = (((num0 * body2.invInertiaWorld.M12) + (num1 * body2.invInertiaWorld.M22)) + (num2 * body2.invInertiaWorld.M32)); FP num5 = (((num0 * body2.invInertiaWorld.M13) + (num1 * body2.invInertiaWorld.M23)) + (num2 * body2.invInertiaWorld.M33)); body2.angularVelocity.x += num3; body2.angularVelocity.y += num4; body2.angularVelocity.z += num5; } } lastTimeStep = timestep; newContact = false; }
/// <summary> /// Initializes a contact. /// </summary> /// <param name="body1">The first body.</param> /// <param name="body2">The second body.</param> /// <param name="point1">The collision point in worldspace</param> /// <param name="point2">The collision point in worldspace</param> /// <param name="n">The normal pointing to body2.</param> /// <param name="penetration">The estimated penetration depth.</param> public void Initialize(RigidBody body1, RigidBody body2, ref TSVector point1, ref TSVector point2, ref TSVector n, FP penetration, bool newContact, ContactSettings settings) { this.body1 = body1; this.body2 = body2; this.normal = n; normal.Normalize(); this.p1 = point1; this.p2 = point2; this.newContact = newContact; TSVector.Subtract(ref p1, ref body1.position, out relativePos1); TSVector.Subtract(ref p2, ref body2.position, out relativePos2); TSVector.Transform(ref relativePos1, ref body1.invOrientation, out realRelPos1); TSVector.Transform(ref relativePos2, ref body2.invOrientation, out realRelPos2); this.initialPen = penetration; this.penetration = penetration; body1IsMassPoint = body1.isParticle; body2IsMassPoint = body2.isParticle; // Material Properties if (newContact) { treatBody1AsStatic = body1.isStatic; treatBody2AsStatic = body2.isStatic; accumulatedNormalImpulse = FP.Zero; accumulatedTangentImpulse = FP.Zero; lostSpeculativeBounce = FP.Zero; switch (settings.MaterialCoefficientMixing) { case ContactSettings.MaterialCoefficientMixingType.TakeMaximum: staticFriction = TSMath.Max(body1.staticFriction, body2.staticFriction); dynamicFriction = TSMath.Max(body1.staticFriction, body2.staticFriction); restitution = TSMath.Max(body1.restitution, body2.restitution); break; case ContactSettings.MaterialCoefficientMixingType.TakeMinimum: staticFriction = TSMath.Min(body1.staticFriction, body2.staticFriction); dynamicFriction = TSMath.Min(body1.staticFriction, body2.staticFriction); restitution = TSMath.Min(body1.restitution, body2.restitution); break; case ContactSettings.MaterialCoefficientMixingType.UseAverage: staticFriction = (body1.staticFriction + body2.staticFriction) * FP.Half; dynamicFriction = (body1.staticFriction + body2.staticFriction) * FP.Half; restitution = (body1.restitution + body2.restitution) * FP.Half; break; } } this.settings = settings; }
public bool UpdateClosestVectorAndPoints() { if (_needsUpdate) { _cachedBC.Reset(); _needsUpdate = false; TSVector p, a, b, c, d; switch (NumVertices) { case 0: _cachedValidClosest = false; break; case 1: _cachedPA = _simplexPointsP[0]; _cachedPB = _simplexPointsQ[0]; _cachedV = _cachedPA - _cachedPB; _cachedBC.Reset(); _cachedBC.SetBarycentricCoordinates(FP.One, FP.Zero, FP.Zero, FP.Zero); _cachedValidClosest = _cachedBC.IsValid; break; case 2: //closest point origin from line segment TSVector from = _simplexVectorW[0]; TSVector to = _simplexVectorW[1]; //TSVector nearest; TSVector diff = TSVector.Negate(from); TSVector v = to - from; FP t = TSVector.Dot(v, diff); if (t > FP.Zero) { FP dotVV = v.sqrMagnitude; if (t < dotVV) { t /= dotVV; diff -= t * v; _cachedBC.usedVertices.UsedVertexA = true; _cachedBC.usedVertices.UsedVertexB = true; } else { t = FP.One; diff -= v; //reduce to 1 point _cachedBC.usedVertices.UsedVertexB = true; } } else { t = FP.Zero; //reduce to 1 point _cachedBC.usedVertices.UsedVertexA = true; } _cachedBC.SetBarycentricCoordinates(FP.One - t, t, FP.Zero, FP.Zero); //nearest = from + t * v; _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]); _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]); _cachedV = _cachedPA - _cachedPB; ReduceVertices(_cachedBC.usedVertices); _cachedValidClosest = _cachedBC.IsValid; break; case 3: //closest point origin from triangle a = _simplexVectorW[0]; b = _simplexVectorW[1]; c = _simplexVectorW[2]; ClosestPtPointTriangle(a, b, c, ref _cachedBC); _cachedPA = _simplexPointsP[0] * _cachedBC.barycentricCoords[0] + _simplexPointsP[1] * _cachedBC.barycentricCoords[1] + _simplexPointsP[2] * _cachedBC.barycentricCoords[2]; _cachedPB = _simplexPointsQ[0] * _cachedBC.barycentricCoords[0] + _simplexPointsQ[1] * _cachedBC.barycentricCoords[1] + _simplexPointsQ[2] * _cachedBC.barycentricCoords[2]; _cachedV = _cachedPA - _cachedPB; ReduceVertices(_cachedBC.usedVertices); _cachedValidClosest = _cachedBC.IsValid; break; case 4: a = _simplexVectorW[0]; b = _simplexVectorW[1]; c = _simplexVectorW[2]; d = _simplexVectorW[3]; bool hasSeparation = ClosestPtPointTetrahedron(a, b, c, d, ref _cachedBC); if (hasSeparation) { _cachedPA = _simplexPointsP[0] * _cachedBC.barycentricCoords[0] + _simplexPointsP[1] * _cachedBC.barycentricCoords[1] + _simplexPointsP[2] * _cachedBC.barycentricCoords[2] + _simplexPointsP[3] * _cachedBC.barycentricCoords[3]; _cachedPB = _simplexPointsQ[0] * _cachedBC.barycentricCoords[0] + _simplexPointsQ[1] * _cachedBC.barycentricCoords[1] + _simplexPointsQ[2] * _cachedBC.barycentricCoords[2] + _simplexPointsQ[3] * _cachedBC.barycentricCoords[3]; _cachedV = _cachedPA - _cachedPB; ReduceVertices(_cachedBC.usedVertices); } else { if (_cachedBC.degenerate) { _cachedValidClosest = false; } else { _cachedValidClosest = true; //degenerate case == false, penetration = true + zero _cachedV.x = _cachedV.y = _cachedV.z = FP.Zero; } break; // !!!!!!!!!!!! proverit na vsakiy sluchai } _cachedValidClosest = _cachedBC.IsValid; //closest point origin from tetrahedron break; default: _cachedValidClosest = false; break; } } return(_cachedValidClosest); }
public void Do(Skill skill, uint targetAgentId, AgentObjectType targetAgentType, TSVector position, TSVector forward) { if (skill == null) { return; } if (!CanDo(skill)) { return; } m_lstCurSkill.Add(skill); skill.Do(targetAgentId, targetAgentType, position, forward); }
/// <summary> /// Checks if given point is within a shape. /// </summary> /// <param name="support">The supportmap implementation representing the shape.</param> /// <param name="orientation">The orientation of the shape.</param> /// <param name="invOrientation">The inverse orientation of the shape.</param> /// <param name="position">The position of the shape.</param> /// <param name="point">The point to check.</param> /// <returns>Returns true if the point is within the shape, otherwise false.</returns> public static bool Pointcast(ISupportMappable support, ref TSMatrix orientation, ref TSVector position, ref TSVector point) { TSVector arbitraryPoint; SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint); TSVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint); TSVector r; support.SupportCenter(out r); TSVector.Transform(ref r, ref orientation, out r); TSVector.Add(ref position, ref r, out r); TSVector.Subtract(ref point, ref r, out r); TSVector x = point; TSVector w, p; FP VdotR; TSVector v; TSVector.Subtract(ref x, ref arbitraryPoint, out v); FP dist = v.sqrMagnitude; FP epsilon = CollideEpsilon; int maxIter = MaxIterations; VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); while ((dist > epsilon) && (maxIter-- != 0)) { SupportMapTransformed(support, ref orientation, ref position, ref v, out p); TSVector.Subtract(ref x, ref p, out w); FP VdotW = TSVector.Dot(ref v, ref w); if (VdotW > FP.Zero) { VdotR = TSVector.Dot(ref v, ref r); if (VdotR >= -(TSMath.Epsilon * TSMath.Epsilon)) { simplexSolverPool.GiveBack(simplexSolver); return(false); } else { simplexSolver.Reset(); } } if (!simplexSolver.InSimplex(w)) { simplexSolver.AddVertex(w, x, p); } if (simplexSolver.Closest(out v)) { dist = v.sqrMagnitude; } else { dist = FP.Zero; } } simplexSolverPool.GiveBack(simplexSolver); return(true); }