public static void CatmullRom(ref TSVector2 value1, ref TSVector2 value2, ref TSVector2 value3, ref TSVector2 value4, FP amount, out TSVector2 result) { result = new TSVector2( TSMath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount), TSMath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount)); }
public static TSVector3 NormalizeCheck(TSVector3 v) { //防止溢出 FP div = TSMath.Max(TSMath.Abs(v.x), TSMath.Abs(v.y), TSMath.Abs(v.z)); return(v / div); }
public static FP Distance(TSVector v1, TSVector v2) { FP result; DistanceSquared(ref v1, ref v2, out result); return(TSMath.Sqrt(result)); }
public static TSQuaternion Slerp(TSQuaternion from, TSQuaternion to, FP t) { t = TSMath.Clamp(t, 0, 1); //t = 0.5f; //if (t == 0) // return from; FP dot = Dot(from, to); if (dot < 0.0f) { to = Multiply(to, -1); dot = -dot; } if (dot >= FP.NearOne) { // If the inputs are too close for comfort, linearly interpolate // and normalize the result. TSQuaternion result = Lerp(from, to, t); return(result); } FP halfTheta = FP.Acos(dot); //UnityEngine.Debug.LogWarning("halfTheta:"+ halfTheta+" dot:"+dot+ " FP.Sin(halfTheta):"+ FP.Sin(halfTheta)); return(Multiply(Multiply(from, FP.Sin((1 - t) * halfTheta)) + Multiply(to, FP.Sin(t * halfTheta)), 1 / FP.Sin(halfTheta))); }
public static void Barycentric(ref TSVector2 value1, ref TSVector2 value2, ref TSVector2 value3, FP amount1, FP amount2, out TSVector2 result) { result = new TSVector2( TSMath.Barycentric(value1.x, value2.x, value3.x, amount1, amount2), TSMath.Barycentric(value1.y, value2.y, value3.y, amount1, amount2)); }
public override void OnSyncedUpdate() { //if (useSpring) //{ // //Adding a spring and damper Term to the Equation of Motion // thisBody.AddTorque ((-1) * TSWorldAxis * ((thisJoint.getHingeAngle () - Spring.tagetPosition) * Spring.spring + thisJoint.getAngularVel () * Spring.damper)); //} if (Input.GetKeyDown(KeyCode.T)) { thisBody.AddTorque(new Vector3(0, 90, 0).ToTSVector()); } Debug.Log(thisJoint.getHingeAngle() + " : " + thisJoint.getAngularVel()); //thisBody.tsTransform.rotation = TSQuaternion.Euler (TSMath.Clamp (thisBody.tsTransform.eulerAngles.x, -30, 30), thisBody.tsTransform.eulerAngles.y, thisBody.tsTransform.eulerAngles.z); //CharacterJoint; if (TSMath.Abs(thisJoint.AppliedImpulse) >= breakForce) //@TODO: Add break torque { thisJoint.Deactivate(); Destroy(this); } }
public static TSVector2 Lerp(TSVector2 value1, TSVector2 value2, FP amount) { amount = TSMath.Clamp(amount, 0, 1); return(new TSVector2( TSMath.Lerp(value1.x, value2.x, amount), TSMath.Lerp(value1.y, value2.y, amount))); }
// The smaller of the two possible angles between the two vectors is returned, therefore the result will never be greater than 180 degrees or smaller than -180 degrees. // If you imagine the from and to vectors as lines on a piece of paper, both originating from the same point, then the /axis/ vector would point up out of the paper. // The measured angle between the two vectors would be positive in a clockwise direction and negative in an anti-clockwise direction. public static FP SignedAngle(TSVector from, TSVector to, TSVector axis) { TSVector fromNorm = from.normalized, toNorm = to.normalized; FP unsignedAngle = TSMath.Acos(TSMath.Clamp(Dot(fromNorm, toNorm), -FP.ONE, FP.ONE)) * TSMath.Rad2Deg; FP sign = TSMath.Sign(Dot(axis, Cross(fromNorm, toNorm))); return(unsignedAngle * sign); }
public override void GetBoundingBox(ref TSMatrix orientation, out TSBBox box) { TSMatrix tSMatrix; TSMath.Absolute(ref orientation, out tSMatrix); TSVector max; TSVector.Transform(ref this.halfSize, ref tSMatrix, out max); box.max = max; TSVector.Negate(ref max, out box.min); }
public void Transform(ref TSMatrix orientation) { TSVector value = FP.Half * (this.max - this.min); TSVector value2 = FP.Half * (this.max + this.min); TSVector.Transform(ref value2, ref orientation, out value2); TSMatrix tSMatrix; TSMath.Absolute(ref orientation, out tSMatrix); TSVector.Transform(ref value, ref tSMatrix, out value); this.max = value2 + value; this.min = value2 - value; }
public override int Prepare(ref TSBBox box) { bool flag = box.min.x < this.boundings.min.x; if (flag) { this.minX = 0; } else { this.minX = (int)((long)FP.Floor((box.min.x - this.sphericalExpansion) / this.scaleX)); this.minX = (int)((long)TSMath.Max(this.minX, 0)); } bool flag2 = box.max.x > this.boundings.max.x; if (flag2) { this.maxX = this.heightsLength0 - 1; } else { this.maxX = (int)((long)FP.Ceiling((box.max.x + this.sphericalExpansion) / this.scaleX)); this.maxX = (int)((long)TSMath.Min(this.maxX, this.heightsLength0 - 1)); } bool flag3 = box.min.z < this.boundings.min.z; if (flag3) { this.minZ = 0; } else { this.minZ = (int)((long)FP.Floor((box.min.z - this.sphericalExpansion) / this.scaleZ)); this.minZ = (int)((long)TSMath.Max(this.minZ, 0)); } bool flag4 = box.max.z > this.boundings.max.z; if (flag4) { this.maxZ = this.heightsLength1 - 1; } else { this.maxZ = (int)((long)FP.Ceiling((box.max.z + this.sphericalExpansion) / this.scaleZ)); this.maxZ = (int)((long)TSMath.Min(this.maxZ, this.heightsLength1 - 1)); } this.numX = this.maxX - this.minX; this.numZ = this.maxZ - this.minZ; return(this.numX * this.numZ * 2); }
public static TSQuaternion Slerp(TSQuaternion from, TSQuaternion to, FP t) { t = TSMath.Clamp(t, 0, 1); FP dot = Dot(from, to); if (dot < 0.0f) { to = Multiply(to, -1); dot = -dot; } FP halfTheta = FP.Acos(dot); return(Multiply(Multiply(from, FP.Sin((1 - t) * halfTheta)) + Multiply(to, FP.Sin(t * halfTheta)), 1 / FP.Sin(halfTheta))); }
public override void Iterate() { bool flag = this.skipConstraint; if (!flag) { FP x = TSVector.Dot(ref this.body1.linearVelocity, ref this.jacobian[0]); x += TSVector.Dot(ref this.body2.linearVelocity, ref this.jacobian[1]); FP y = this.accumulatedImpulse * this.softnessOverDt; FP fP = -this.effectiveMass * (x + this.bias + y); bool flag2 = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMinimumDistance; if (flag2) { FP y2 = this.accumulatedImpulse; this.accumulatedImpulse = TSMath.Max(this.accumulatedImpulse + fP, 0); fP = this.accumulatedImpulse - y2; } else { bool flag3 = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMaximumDistance; if (flag3) { FP y3 = this.accumulatedImpulse; this.accumulatedImpulse = TSMath.Min(this.accumulatedImpulse + fP, 0); fP = this.accumulatedImpulse - y3; } else { this.accumulatedImpulse += fP; } } bool flag4 = !this.body1.isStatic; if (flag4) { TSVector tSVector; TSVector.Multiply(ref this.jacobian[0], fP * this.body1.inverseMass, out tSVector); TSVector.Add(ref tSVector, ref this.body1.linearVelocity, out this.body1.linearVelocity); } bool flag5 = !this.body2.isStatic; if (flag5) { TSVector tSVector; TSVector.Multiply(ref this.jacobian[1], fP * this.body2.inverseMass, out tSVector); TSVector.Add(ref tSVector, ref this.body2.linearVelocity, out this.body2.linearVelocity); } } }
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; this.normal.Normalize(); this.p1 = point1; this.p2 = point2; this.newContact = newContact; TSVector.Subtract(ref this.p1, ref body1.position, out this.relativePos1); TSVector.Subtract(ref this.p2, ref body2.position, out this.relativePos2); TSVector.Transform(ref this.relativePos1, ref body1.invOrientation, out this.realRelPos1); TSVector.Transform(ref this.relativePos2, ref body2.invOrientation, out this.realRelPos2); this.initialPen = penetration; this.penetration = penetration; this.body1IsMassPoint = body1.isParticle; this.body2IsMassPoint = body2.isParticle; if (newContact) { this.treatBody1AsStatic = body1.isStatic; this.treatBody2AsStatic = body2.isStatic; this.accumulatedNormalImpulse = FP.Zero; this.accumulatedTangentImpulse = FP.Zero; this.lostSpeculativeBounce = FP.Zero; switch (settings.MaterialCoefficientMixing) { case ContactSettings.MaterialCoefficientMixingType.TakeMaximum: this.staticFriction = TSMath.Max(body1.material.staticFriction, body2.material.staticFriction); this.dynamicFriction = TSMath.Max(body1.material.kineticFriction, body2.material.kineticFriction); this.restitution = TSMath.Max(body1.material.restitution, body2.material.restitution); break; case ContactSettings.MaterialCoefficientMixingType.TakeMinimum: this.staticFriction = TSMath.Min(body1.material.staticFriction, body2.material.staticFriction); this.dynamicFriction = TSMath.Min(body1.material.kineticFriction, body2.material.kineticFriction); this.restitution = TSMath.Min(body1.material.restitution, body2.material.restitution); break; case ContactSettings.MaterialCoefficientMixingType.UseAverage: this.staticFriction = (body1.material.staticFriction + body2.material.staticFriction) / (2 * FP.One); this.dynamicFriction = (body1.material.kineticFriction + body2.material.kineticFriction) / (2 * FP.One); this.restitution = (body1.material.restitution + body2.material.restitution) / (2 * FP.One); break; } } this.settings = settings; }
public override void GetBoundingBox(ref TSMatrix orientation, out TSBBox box) { box.min = this.mInternalBBox.min; box.max = this.mInternalBBox.max; TSVector tSVector = FP.Half * (box.max - box.min); TSVector tSVector2 = FP.Half * (box.max + box.min); TSVector value; TSVector.Transform(ref tSVector2, ref orientation, out value); TSMatrix tSMatrix; TSMath.Absolute(ref orientation, out tSMatrix); TSVector value2; TSVector.Transform(ref tSVector, ref tSMatrix, out value2); box.max = value + value2; box.min = value - value2; }
public override void Iterate() { bool flag = this.skipConstraint; if (!flag) { FP x = this.body1.linearVelocity * this.jacobian[0] + this.body1.angularVelocity * this.jacobian[1] + this.body2.linearVelocity * this.jacobian[2] + this.body2.angularVelocity * this.jacobian[3]; FP y = this.accumulatedImpulse * this.softnessOverDt; FP fP = -this.effectiveMass * (x + this.bias + y); bool flag2 = this.behavior == PointPointDistance.DistanceBehavior.LimitMinimumDistance; if (flag2) { FP y2 = this.accumulatedImpulse; this.accumulatedImpulse = TSMath.Max(this.accumulatedImpulse + fP, 0); fP = this.accumulatedImpulse - y2; } else { bool flag3 = this.behavior == PointPointDistance.DistanceBehavior.LimitMaximumDistance; if (flag3) { FP y3 = this.accumulatedImpulse; this.accumulatedImpulse = TSMath.Min(this.accumulatedImpulse + fP, 0); fP = this.accumulatedImpulse - y3; } else { this.accumulatedImpulse += fP; } } bool flag4 = !this.body1.isStatic; if (flag4) { this.body1.linearVelocity += this.body1.inverseMass * fP * this.jacobian[0]; this.body1.angularVelocity += TSVector.Transform(fP * this.jacobian[1], this.body1.invInertiaWorld); } bool flag5 = !this.body2.isStatic; if (flag5) { this.body2.linearVelocity += this.body2.inverseMass * fP * this.jacobian[2]; this.body2.angularVelocity += TSVector.Transform(fP * this.jacobian[3], this.body2.invInertiaWorld); } } }
public override void PrepareForIteration(FP timestep) { this.effectiveMass = this.body1.invInertiaWorld + this.body2.invInertiaWorld; this.softnessOverDt = this.softness / timestep; this.effectiveMass.M11 = this.effectiveMass.M11 + this.softnessOverDt; this.effectiveMass.M22 = this.effectiveMass.M22 + this.softnessOverDt; this.effectiveMass.M33 = this.effectiveMass.M33 + this.softnessOverDt; TSMatrix.Inverse(ref this.effectiveMass, out this.effectiveMass); TSMatrix value; TSMatrix.Multiply(ref this.initialOrientation1, ref this.initialOrientation2, out value); TSMatrix.Transpose(ref value, out value); TSMatrix tSMatrix = value * this.body2.invOrientation * this.body1.orientation; FP fP = tSMatrix.M32 - tSMatrix.M23; FP fP2 = tSMatrix.M13 - tSMatrix.M31; FP fP3 = tSMatrix.M21 - tSMatrix.M12; FP fP4 = TSMath.Sqrt(fP * fP + fP2 * fP2 + fP3 * fP3); FP x = tSMatrix.M11 + tSMatrix.M22 + tSMatrix.M33; FP value2 = FP.Atan2(fP4, x - 1); TSVector value3 = new TSVector(fP, fP2, fP3) * value2; bool flag = fP4 != FP.Zero; if (flag) { value3 *= FP.One / fP4; } this.bias = value3 * this.biasFactor * (-FP.One / timestep); bool flag2 = !this.body1.IsStatic; if (flag2) { this.body1.angularVelocity += TSVector.Transform(this.accumulatedImpulse, this.body1.invInertiaWorld); } bool flag3 = !this.body2.IsStatic; if (flag3) { this.body2.angularVelocity += TSVector.Transform(-FP.One * this.accumulatedImpulse, this.body2.invInertiaWorld); } }
internal void InverseTransform(ref TSVector position, ref TSMatrix orientation) { TSVector.Subtract(ref this.max, ref position, out this.max); TSVector.Subtract(ref this.min, ref position, out this.min); TSVector tSVector; TSVector.Add(ref this.max, ref this.min, out tSVector); tSVector.x *= FP.Half; tSVector.y *= FP.Half; tSVector.z *= FP.Half; TSVector tSVector2; TSVector.Subtract(ref this.max, ref this.min, out tSVector2); tSVector2.x *= FP.Half; tSVector2.y *= FP.Half; tSVector2.z *= FP.Half; TSVector.TransposedTransform(ref tSVector, ref orientation, out tSVector); TSMatrix tSMatrix; TSMath.Absolute(ref orientation, out tSMatrix); TSVector.TransposedTransform(ref tSVector2, ref tSMatrix, out tSVector2); TSVector.Add(ref tSVector, ref tSVector2, out this.max); TSVector.Subtract(ref tSVector, ref tSVector2, out this.min); }
public static TSVector2 Min(TSVector2 value1, TSVector2 value2) { return(new TSVector2( TSMath.Min(value1.x, value2.x), TSMath.Min(value1.y, value2.y))); }
public static TSVector2 CatmullRom(TSVector2 value1, TSVector2 value2, TSVector2 value3, TSVector2 value4, FP amount) { return(new TSVector2( TSMath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount), TSMath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount))); }
// Returns the angle in degrees between /from/ and /to/. This is always the smallest public static FP Angle(TSVector from, TSVector to) { return(TSMath.Acos(TSMath.Clamp(Dot(from.normalized, to.normalized), -FP.ONE, FP.ONE)) * TSMath.Rad2Deg); }
public static TSVector2 Clamp(TSVector2 value1, TSVector2 min, TSVector2 max) { return(new TSVector2( TSMath.Clamp(value1.x, min.x, max.x), TSMath.Clamp(value1.y, min.y, max.y))); }
public virtual void MakeHull(ref List <TSVector> triangleList, int generationThreshold) { FP zero = FP.Zero; bool flag = generationThreshold < 0; if (flag) { generationThreshold = 4; } Stack <Shape.ClipTriangle> stack = new Stack <Shape.ClipTriangle>(); TSVector[] array = new TSVector[] { new TSVector(-1, 0, 0), new TSVector(1, 0, 0), new TSVector(0, -1, 0), new TSVector(0, 1, 0), new TSVector(0, 0, -1), new TSVector(0, 0, 1) }; int[,] array2 = new int[, ] { { 5, 1, 3 }, { 4, 3, 1 }, { 3, 4, 0 }, { 0, 5, 3 }, { 5, 2, 1 }, { 4, 1, 2 }, { 2, 0, 4 }, { 0, 2, 5 } }; for (int i = 0; i < 8; i++) { stack.Push(new Shape.ClipTriangle { n1 = array[array2[i, 0]], n2 = array[array2[i, 1]], n3 = array[array2[i, 2]], generation = 0 }); } while (stack.Count > 0) { Shape.ClipTriangle clipTriangle = stack.Pop(); TSVector tSVector; this.SupportMapping(ref clipTriangle.n1, out tSVector); TSVector tSVector2; this.SupportMapping(ref clipTriangle.n2, out tSVector2); TSVector tSVector3; this.SupportMapping(ref clipTriangle.n3, out tSVector3); FP sqrMagnitude = (tSVector2 - tSVector).sqrMagnitude; FP sqrMagnitude2 = (tSVector3 - tSVector2).sqrMagnitude; FP sqrMagnitude3 = (tSVector - tSVector3).sqrMagnitude; bool flag2 = TSMath.Max(TSMath.Max(sqrMagnitude, sqrMagnitude2), sqrMagnitude3) > zero && clipTriangle.generation < generationThreshold; if (flag2) { Shape.ClipTriangle item = default(Shape.ClipTriangle); Shape.ClipTriangle item2 = default(Shape.ClipTriangle); Shape.ClipTriangle item3 = default(Shape.ClipTriangle); Shape.ClipTriangle item4 = default(Shape.ClipTriangle); item.generation = clipTriangle.generation + 1; item2.generation = clipTriangle.generation + 1; item3.generation = clipTriangle.generation + 1; item4.generation = clipTriangle.generation + 1; item.n1 = clipTriangle.n1; item2.n2 = clipTriangle.n2; item3.n3 = clipTriangle.n3; TSVector tSVector4 = FP.Half * (clipTriangle.n1 + clipTriangle.n2); tSVector4.Normalize(); item.n2 = tSVector4; item2.n1 = tSVector4; item4.n3 = tSVector4; tSVector4 = FP.Half * (clipTriangle.n2 + clipTriangle.n3); tSVector4.Normalize(); item2.n3 = tSVector4; item3.n2 = tSVector4; item4.n1 = tSVector4; tSVector4 = FP.Half * (clipTriangle.n3 + clipTriangle.n1); tSVector4.Normalize(); item.n3 = tSVector4; item3.n1 = tSVector4; item4.n2 = tSVector4; stack.Push(item); stack.Push(item2); stack.Push(item3); stack.Push(item4); } else { bool flag3 = ((tSVector3 - tSVector) % (tSVector2 - tSVector)).sqrMagnitude > TSMath.Epsilon; if (flag3) { triangleList.Add(tSVector); triangleList.Add(tSVector2); triangleList.Add(tSVector3); } } } }
public static void SmoothStep(ref TSVector2 value1, ref TSVector2 value2, FP amount, out TSVector2 result) { result = new TSVector2( TSMath.SmoothStep(value1.x, value2.x, amount), TSMath.SmoothStep(value1.y, value2.y, amount)); }
public static TSVector2 SmoothStep(TSVector2 value1, TSVector2 value2, FP amount) { return(new TSVector2( TSMath.SmoothStep(value1.x, value2.x, amount), TSMath.SmoothStep(value1.y, value2.y, amount))); }
public static void Min(ref TSVector2 value1, ref TSVector2 value2, out TSVector2 result) { result.x = TSMath.Min(value1.x, value2.x); result.y = TSMath.Min(value1.y, value2.y); }
public static void Clamp(ref TSVector2 value1, ref TSVector2 min, ref TSVector2 max, out TSVector2 result) { result = new TSVector2( TSMath.Clamp(value1.x, min.x, max.x), TSMath.Clamp(value1.y, min.y, max.y)); }
public static void LerpUnclamped(ref TSVector2 value1, ref TSVector2 value2, FP amount, out TSVector2 result) { result = new TSVector2( TSMath.Lerp(value1.x, value2.x, amount), TSMath.Lerp(value1.y, value2.y, amount)); }
public static TSVector2 LerpUnclamped(TSVector2 value1, TSVector2 value2, FP amount) { return(new TSVector2( TSMath.Lerp(value1.x, value2.x, amount), TSMath.Lerp(value1.y, value2.y, amount))); }
public static void Hermite(ref TSVector2 value1, ref TSVector2 tangent1, ref TSVector2 value2, ref TSVector2 tangent2, FP amount, out TSVector2 result) { result.x = TSMath.Hermite(value1.x, tangent1.x, value2.x, tangent2.x, amount); result.y = TSMath.Hermite(value1.y, tangent1.y, value2.y, tangent2.y, amount); }