public static TSVector operator %(TSVector value1, TSVector value2) { TSVector result; TSVector.Cross(ref value1, ref value2, out result); return(result); }
/// <summary> /// The cross product of two vectors. /// </summary> /// <param name="vector1">The first vector.</param> /// <param name="vector2">The second vector.</param> /// <returns>The cross product of both vectors.</returns> #region public static JVector Cross(JVector vector1, JVector vector2) public static TSVector Cross(TSVector vector1, TSVector vector2) { TSVector result; TSVector.Cross(ref vector1, ref vector2, out result); return(result); }
public void AddForce(TSVector force, TSVector pos) { TSVector.Add(ref this.force, ref force, out this.force); TSVector.Subtract(ref pos, ref this.position, out pos); TSVector.Cross(ref pos, ref force, out pos); TSVector.Add(ref pos, ref this.torque, out this.torque); }
public void GetNormal(out TSVector normal) { TSVector tSVector; TSVector.Subtract(ref this.owner.points[this.indices.I1].position, ref this.owner.points[this.indices.I0].position, out tSVector); TSVector.Subtract(ref this.owner.points[this.indices.I2].position, ref this.owner.points[this.indices.I0].position, out normal); TSVector.Cross(ref tSVector, ref normal, out normal); }
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector) { TSVector tSVector = TSVector.Cross(fromVector, toVector); TSQuaternion result = new TSQuaternion(tSVector.x, tSVector.y, tSVector.z, TSVector.Dot(fromVector, toVector)); result.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude); result.Normalize(); return(result); }
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector) { TSVector w = TSVector.Cross(fromVector, toVector); TSQuaternion q = new TSQuaternion(w.x, w.y, w.z, TSVector.Dot(fromVector, toVector)); q.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude); q.Normalize(); return(q); }
public void ApplyImpulse(TSVector impulse, TSVector relativePosition) { bool flag = this.isStatic; if (flag) { throw new InvalidOperationException("Can't apply an impulse to a static body."); } TSVector tSVector; TSVector.Multiply(ref impulse, this.inverseMass, out tSVector); TSVector.Add(ref this.linearVelocity, ref tSVector, out this.linearVelocity); TSVector.Cross(ref relativePosition, ref impulse, out tSVector); TSVector.Transform(ref tSVector, ref this.invInertiaWorld, out tSVector); TSVector.Add(ref this.angularVelocity, ref tSVector, out this.angularVelocity); }
public int PointOutsideOfPlane(TSVector p, TSVector a, TSVector b, TSVector c, TSVector d) { TSVector vector = TSVector.Cross(b - a, c - a); FP x = TSVector.Dot(p - a, vector); FP fP = TSVector.Dot(d - a, vector); bool flag = fP * fP < FP.EN8; int result; if (flag) { result = -1; } else { result = ((x * fP < FP.Zero) ? 1 : 0); } return(result); }
public static void LookAt(out TSMatrix result, TSVector position, TSVector target) { TSVector tSVector = target - position; tSVector.Normalize(); TSVector tSVector2 = TSVector.Cross(TSVector.up, tSVector); tSVector2.Normalize(); TSVector tSVector3 = TSVector.Cross(tSVector, tSVector2); result.M11 = tSVector2.x; result.M21 = tSVector3.x; result.M31 = tSVector.x; result.M12 = tSVector2.y; result.M22 = tSVector3.y; result.M32 = tSVector.y; result.M13 = tSVector2.z; result.M23 = tSVector3.z; result.M33 = tSVector.z; }
public override void SetCurrentShape(int index) { this.vecs[0] = this.octree.GetVertex(this.octree.tris[this.potentialTriangles[index]].I0); this.vecs[1] = this.octree.GetVertex(this.octree.tris[this.potentialTriangles[index]].I1); this.vecs[2] = this.octree.GetVertex(this.octree.tris[this.potentialTriangles[index]].I2); TSVector geomCen = this.vecs[0]; TSVector.Add(ref geomCen, ref this.vecs[1], out geomCen); TSVector.Add(ref geomCen, ref this.vecs[2], out geomCen); TSVector.Multiply(ref geomCen, FP.One / (3 * FP.One), out geomCen); this.geomCen = geomCen; TSVector.Subtract(ref this.vecs[1], ref this.vecs[0], out geomCen); TSVector.Subtract(ref this.vecs[2], ref this.vecs[0], out this.normal); TSVector.Cross(ref geomCen, ref this.normal, out this.normal); bool flag = this.flipNormal; if (flag) { this.normal.Negate(); } }
public override void SetCurrentShape(int index) { bool flag = false; bool flag2 = index >= this.numX * this.numZ; if (flag2) { flag = true; index -= this.numX * this.numZ; } int num = index % this.numX; int num2 = index / this.numX; bool flag3 = flag; if (flag3) { this.points[0].Set((this.minX + num) * this.scaleX, this.heights[this.minX + num, this.minZ + num2], (this.minZ + num2) * this.scaleZ); this.points[1].Set((this.minX + num + 1) * this.scaleX, this.heights[this.minX + num + 1, this.minZ + num2], (this.minZ + num2) * this.scaleZ); this.points[2].Set((this.minX + num) * this.scaleX, this.heights[this.minX + num, this.minZ + num2 + 1], (this.minZ + num2 + 1) * this.scaleZ); } else { this.points[0].Set((this.minX + num + 1) * this.scaleX, this.heights[this.minX + num + 1, this.minZ + num2], (this.minZ + num2) * this.scaleZ); this.points[1].Set((this.minX + num + 1) * this.scaleX, this.heights[this.minX + num + 1, this.minZ + num2 + 1], (this.minZ + num2 + 1) * this.scaleZ); this.points[2].Set((this.minX + num) * this.scaleX, this.heights[this.minX + num, this.minZ + num2 + 1], (this.minZ + num2 + 1) * this.scaleZ); } TSVector geomCen = this.points[0]; TSVector.Add(ref geomCen, ref this.points[1], out geomCen); TSVector.Add(ref geomCen, ref this.points[2], out geomCen); TSVector.Multiply(ref geomCen, FP.One / (3 * FP.One), out geomCen); this.geomCen = geomCen; TSVector.Subtract(ref this.points[1], ref this.points[0], out geomCen); TSVector.Subtract(ref this.points[2], ref this.points[0], out this.normal); TSVector.Cross(ref geomCen, ref this.normal, out this.normal); }
/** * @brief Returns the velocity of the body at some position in world space. **/ public TSVector GetPointVelocity(TSVector worldPoint) { TSVector directionPoint = position - tsCollider.Body.TSPosition; return(TSVector.Cross(tsCollider.Body.TSAngularVelocity, directionPoint) + tsCollider.Body.TSLinearVelocity); }
private int SortCachedPoints(ref TSVector realRelPos1, FP pen) { int num = -1; FP y = pen; for (int i = 0; i < 4; i++) { bool flag = this.contactList[i].penetration > y; if (flag) { num = i; y = this.contactList[i].penetration; } } FP x = 0; FP y2 = 0; FP z = 0; FP w = 0; bool flag2 = num != 0; if (flag2) { TSVector tSVector; TSVector.Subtract(ref realRelPos1, ref this.contactList[1].relativePos1, out tSVector); TSVector tSVector2; TSVector.Subtract(ref this.contactList[3].relativePos1, ref this.contactList[2].relativePos1, out tSVector2); TSVector tSVector3; TSVector.Cross(ref tSVector, ref tSVector2, out tSVector3); x = tSVector3.sqrMagnitude; } bool flag3 = num != 1; if (flag3) { TSVector tSVector4; TSVector.Subtract(ref realRelPos1, ref this.contactList[0].relativePos1, out tSVector4); TSVector tSVector5; TSVector.Subtract(ref this.contactList[3].relativePos1, ref this.contactList[2].relativePos1, out tSVector5); TSVector tSVector6; TSVector.Cross(ref tSVector4, ref tSVector5, out tSVector6); y2 = tSVector6.sqrMagnitude; } bool flag4 = num != 2; if (flag4) { TSVector tSVector7; TSVector.Subtract(ref realRelPos1, ref this.contactList[0].relativePos1, out tSVector7); TSVector tSVector8; TSVector.Subtract(ref this.contactList[3].relativePos1, ref this.contactList[1].relativePos1, out tSVector8); TSVector tSVector9; TSVector.Cross(ref tSVector7, ref tSVector8, out tSVector9); z = tSVector9.sqrMagnitude; } bool flag5 = num != 3; if (flag5) { TSVector tSVector10; TSVector.Subtract(ref realRelPos1, ref this.contactList[0].relativePos1, out tSVector10); TSVector tSVector11; TSVector.Subtract(ref this.contactList[2].relativePos1, ref this.contactList[1].relativePos1, out tSVector11); TSVector tSVector12; TSVector.Cross(ref tSVector10, ref tSVector11, out tSVector12); w = tSVector12.sqrMagnitude; } return(Arbiter.MaxAxis(x, y2, z, w)); }
/** * @brief Returns the velocity of the body at some position in world space. **/ public TSVector2 GetPointVelocity(TSVector2 worldPoint) { TSVector directionPoint = (position - tsCollider.Body.TSPosition).ToTSVector(); return(TSVector.Cross(new TSVector(0, 0, tsCollider.Body.TSAngularVelocity), directionPoint).ToTSVector2() + tsCollider.Body.TSLinearVelocity); }
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2, out TSVector point, out TSVector normal, out FP penetration) { TSVector zero = TSVector.zero; TSVector zero2 = TSVector.zero; TSVector zero3 = TSVector.zero; point = (normal = TSVector.zero); penetration = FP.Zero; TSVector tSVector; support1.SupportCenter(out tSVector); TSVector.Transform(ref tSVector, ref orientation1, out tSVector); TSVector.Add(ref position1, ref tSVector, out tSVector); TSVector tSVector2; support2.SupportCenter(out tSVector2); TSVector.Transform(ref tSVector2, ref orientation2, out tSVector2); TSVector.Add(ref position2, ref tSVector2, out tSVector2); TSVector tSVector3; TSVector.Subtract(ref tSVector2, ref tSVector, out tSVector3); bool flag = tSVector3.IsNearlyZero(); if (flag) { tSVector3 = new TSVector(FP.EN4, 0, 0); } TSVector tSVector4 = tSVector3; TSVector.Negate(ref tSVector3, out normal); TSVector tSVector5; XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector5); TSVector tSVector6; XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector6); TSVector tSVector7; TSVector.Subtract(ref tSVector6, ref tSVector5, out tSVector7); bool flag2 = TSVector.Dot(ref tSVector7, ref normal) <= FP.Zero; bool result; if (flag2) { result = false; } else { TSVector.Cross(ref tSVector7, ref tSVector3, out normal); bool flag3 = normal.IsNearlyZero(); if (flag3) { TSVector.Subtract(ref tSVector7, ref tSVector3, out normal); normal.Normalize(); point = tSVector5; TSVector.Add(ref point, ref tSVector6, out point); TSVector.Multiply(ref point, FP.Half, out point); TSVector tSVector8; TSVector.Subtract(ref tSVector6, ref tSVector5, out tSVector8); penetration = TSVector.Dot(ref tSVector8, ref normal); result = true; } else { TSVector.Negate(ref normal, out tSVector4); TSVector tSVector9; XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector9); TSVector tSVector10; XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector10); TSVector tSVector11; TSVector.Subtract(ref tSVector10, ref tSVector9, out tSVector11); bool flag4 = TSVector.Dot(ref tSVector11, ref normal) <= FP.Zero; if (flag4) { result = false; } else { TSVector tSVector8; TSVector.Subtract(ref tSVector7, ref tSVector3, out tSVector8); TSVector tSVector12; TSVector.Subtract(ref tSVector11, ref tSVector3, out tSVector12); TSVector.Cross(ref tSVector8, ref tSVector12, out normal); FP x = TSVector.Dot(ref normal, ref tSVector3); bool flag5 = x > FP.Zero; if (flag5) { TSVector.Swap(ref tSVector7, ref tSVector11); TSVector.Swap(ref tSVector5, ref tSVector9); TSVector.Swap(ref tSVector6, ref tSVector10); TSVector.Negate(ref normal, out normal); Debug.Log("normal: " + normal); } int num = 0; int num2 = 0; bool flag6 = false; while (true) { bool flag7 = num2 > 34; if (flag7) { break; } num2++; TSVector.Negate(ref normal, out tSVector4); TSVector tSVector13; XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector13); TSVector tSVector14; XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector14); TSVector tSVector15; TSVector.Subtract(ref tSVector14, ref tSVector13, out tSVector15); bool flag8 = TSVector.Dot(ref tSVector15, ref normal) <= FP.Zero; if (flag8) { goto Block_7; } TSVector.Cross(ref tSVector7, ref tSVector15, out tSVector8); bool flag9 = TSVector.Dot(ref tSVector8, ref tSVector3) < FP.Zero; if (flag9) { tSVector11 = tSVector15; tSVector9 = tSVector13; tSVector10 = tSVector14; TSVector.Subtract(ref tSVector7, ref tSVector3, out tSVector8); TSVector.Subtract(ref tSVector15, ref tSVector3, out tSVector12); TSVector.Cross(ref tSVector8, ref tSVector12, out normal); } else { TSVector.Cross(ref tSVector15, ref tSVector11, out tSVector8); bool flag10 = TSVector.Dot(ref tSVector8, ref tSVector3) < FP.Zero; if (!flag10) { goto IL_385; } tSVector7 = tSVector15; tSVector5 = tSVector13; tSVector6 = tSVector14; TSVector.Subtract(ref tSVector15, ref tSVector3, out tSVector8); TSVector.Subtract(ref tSVector11, ref tSVector3, out tSVector12); TSVector.Cross(ref tSVector8, ref tSVector12, out normal); } } result = false; return(result); Block_7: result = false; return(result); IL_385: while (true) { num++; TSVector.Subtract(ref tSVector11, ref tSVector7, out tSVector8); TSVector tSVector15; TSVector.Subtract(ref tSVector15, ref tSVector7, out tSVector12); TSVector.Cross(ref tSVector8, ref tSVector12, out normal); bool flag11 = normal.IsNearlyZero(); if (flag11) { break; } normal.Normalize(); FP x2 = TSVector.Dot(ref normal, ref tSVector7); bool flag12 = x2 >= 0 && !flag6; if (flag12) { flag6 = true; } TSVector.Negate(ref normal, out tSVector4); XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out zero); XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out zero2); TSVector.Subtract(ref zero2, ref zero, out zero3); TSVector.Subtract(ref zero3, ref tSVector15, out tSVector8); FP x3 = TSVector.Dot(ref tSVector8, ref normal); penetration = TSVector.Dot(ref zero3, ref normal); bool flag13 = x3 <= XenoCollide.CollideEpsilon || penetration <= FP.Zero || num > 34; if (flag13) { goto Block_15; } TSVector.Cross(ref zero3, ref tSVector3, out tSVector8); FP x4 = TSVector.Dot(ref tSVector8, ref tSVector7); bool flag14 = x4 >= FP.Zero; if (flag14) { x4 = TSVector.Dot(ref tSVector8, ref tSVector11); bool flag15 = x4 >= FP.Zero; if (flag15) { tSVector7 = zero3; tSVector5 = zero; tSVector6 = zero2; } else { tSVector15 = zero3; TSVector tSVector13 = zero; TSVector tSVector14 = zero2; } } else { x4 = TSVector.Dot(ref tSVector8, ref tSVector15); bool flag16 = x4 >= FP.Zero; if (flag16) { tSVector11 = zero3; tSVector9 = zero; tSVector10 = zero2; } else { tSVector7 = zero3; tSVector5 = zero; tSVector6 = zero2; } } } result = true; return(result); Block_15: bool flag17 = flag6; if (flag17) { TSVector.Cross(ref tSVector7, ref tSVector11, out tSVector8); TSVector tSVector15; FP fP = TSVector.Dot(ref tSVector8, ref tSVector15); TSVector.Cross(ref tSVector15, ref tSVector11, out tSVector8); FP fP2 = TSVector.Dot(ref tSVector8, ref tSVector3); TSVector.Cross(ref tSVector3, ref tSVector7, out tSVector8); FP fP3 = TSVector.Dot(ref tSVector8, ref tSVector15); TSVector.Cross(ref tSVector11, ref tSVector7, out tSVector8); FP fP4 = TSVector.Dot(ref tSVector8, ref tSVector3); FP fP5 = fP + fP2 + fP3 + fP4; bool flag18 = fP5 <= 0; if (flag18) { fP = 0; TSVector.Cross(ref tSVector11, ref tSVector15, out tSVector8); fP2 = TSVector.Dot(ref tSVector8, ref normal); TSVector.Cross(ref tSVector15, ref tSVector7, out tSVector8); fP3 = TSVector.Dot(ref tSVector8, ref normal); TSVector.Cross(ref tSVector7, ref tSVector11, out tSVector8); fP4 = TSVector.Dot(ref tSVector8, ref normal); fP5 = fP2 + fP3 + fP4; } FP x5 = FP.One / fP5; TSVector.Multiply(ref tSVector, fP, out point); TSVector.Multiply(ref tSVector5, fP2, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector.Multiply(ref tSVector9, fP3, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector tSVector13; TSVector.Multiply(ref tSVector13, fP4, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector.Multiply(ref tSVector2, fP, out tSVector12); TSVector.Add(ref tSVector12, ref point, out point); TSVector.Multiply(ref tSVector6, fP2, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector.Multiply(ref tSVector10, fP3, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector tSVector14; TSVector.Multiply(ref tSVector14, fP4, out tSVector8); TSVector.Add(ref point, ref tSVector8, out point); TSVector.Multiply(ref point, x5 * FP.Half, out point); } result = flag6; } } } return(result); }