PhysicsVector.cs 5.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167
  1. /*
  2. * Copyright (c) Contributors, http://opensimulator.org/
  3. * See CONTRIBUTORS.TXT for a full list of copyright holders.
  4. *
  5. * Redistribution and use in source and binary forms, with or without
  6. * modification, are permitted provided that the following conditions are met:
  7. * * Redistributions of source code must retain the above copyright
  8. * notice, this list of conditions and the following disclaimer.
  9. * * Redistributions in binary form must reproduce the above copyright
  10. * notice, this list of conditions and the following disclaimer in the
  11. * documentation and/or other materials provided with the distribution.
  12. * * Neither the name of the OpenSim Project nor the
  13. * names of its contributors may be used to endorse or promote products
  14. * derived from this software without specific prior written permission.
  15. *
  16. * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
  17. * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  18. * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19. * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
  20. * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  21. * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  22. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  23. * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  24. * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  25. * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26. */
  27. using System;
  28. namespace OpenSim.Region.Physics.Manager
  29. {
  30. public class PhysicsVector
  31. {
  32. public float X;
  33. public float Y;
  34. public float Z;
  35. public PhysicsVector()
  36. {
  37. }
  38. public PhysicsVector(float x, float y, float z)
  39. {
  40. X = x;
  41. Y = y;
  42. Z = z;
  43. }
  44. public void setValues(float x, float y, float z)
  45. {
  46. X = x;
  47. Y = y;
  48. Z = z;
  49. }
  50. public static readonly PhysicsVector Zero = new PhysicsVector(0f, 0f, 0f);
  51. public override string ToString()
  52. {
  53. return "<" + X + "," + Y + "," + Z + ">";
  54. }
  55. /// <summary>
  56. /// These routines are the easiest way to store XYZ values in an LLVector3 without requiring 3 calls.
  57. /// </summary>
  58. /// <returns></returns>
  59. public byte[] GetBytes()
  60. {
  61. byte[] byteArray = new byte[12];
  62. Buffer.BlockCopy(BitConverter.GetBytes(X), 0, byteArray, 0, 4);
  63. Buffer.BlockCopy(BitConverter.GetBytes(Y), 0, byteArray, 4, 4);
  64. Buffer.BlockCopy(BitConverter.GetBytes(Z), 0, byteArray, 8, 4);
  65. if (!BitConverter.IsLittleEndian)
  66. {
  67. Array.Reverse(byteArray, 0, 4);
  68. Array.Reverse(byteArray, 4, 4);
  69. Array.Reverse(byteArray, 8, 4);
  70. }
  71. return byteArray;
  72. }
  73. public void FromBytes(byte[] byteArray, int pos)
  74. {
  75. byte[] conversionBuffer = null;
  76. if (!BitConverter.IsLittleEndian)
  77. {
  78. // Big endian architecture
  79. if (conversionBuffer == null)
  80. conversionBuffer = new byte[12];
  81. Buffer.BlockCopy(byteArray, pos, conversionBuffer, 0, 12);
  82. Array.Reverse(conversionBuffer, 0, 4);
  83. Array.Reverse(conversionBuffer, 4, 4);
  84. Array.Reverse(conversionBuffer, 8, 4);
  85. X = BitConverter.ToSingle(conversionBuffer, 0);
  86. Y = BitConverter.ToSingle(conversionBuffer, 4);
  87. Z = BitConverter.ToSingle(conversionBuffer, 8);
  88. }
  89. else
  90. {
  91. // Little endian architecture
  92. X = BitConverter.ToSingle(byteArray, pos);
  93. Y = BitConverter.ToSingle(byteArray, pos + 4);
  94. Z = BitConverter.ToSingle(byteArray, pos + 8);
  95. }
  96. }
  97. // Operations
  98. public static PhysicsVector operator +(PhysicsVector a, PhysicsVector b)
  99. {
  100. return new PhysicsVector(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
  101. }
  102. public static PhysicsVector operator -(PhysicsVector a, PhysicsVector b)
  103. {
  104. return new PhysicsVector(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
  105. }
  106. public static PhysicsVector cross(PhysicsVector a, PhysicsVector b)
  107. {
  108. return new PhysicsVector(a.Y*b.Z - a.Z*b.Y, a.Z*b.X - a.X*b.Z, a.X*b.Y - a.Y*b.X);
  109. }
  110. public float length()
  111. {
  112. return (float) Math.Sqrt(X*X + Y*Y + Z*Z);
  113. }
  114. public static float GetDistanceTo(PhysicsVector a, PhysicsVector b)
  115. {
  116. float dx = a.X - b.X;
  117. float dy = a.Y - b.Y;
  118. float dz = a.Z - b.Z;
  119. return (float) Math.Sqrt(dx * dx + dy * dy + dz * dz);
  120. }
  121. public static PhysicsVector operator /(PhysicsVector v, float f)
  122. {
  123. return new PhysicsVector(v.X/f, v.Y/f, v.Z/f);
  124. }
  125. public static PhysicsVector operator *(PhysicsVector v, float f)
  126. {
  127. return new PhysicsVector(v.X*f, v.Y*f, v.Z*f);
  128. }
  129. public static PhysicsVector operator *(float f, PhysicsVector v)
  130. {
  131. return v*f;
  132. }
  133. public virtual bool IsIdentical(PhysicsVector v, float tolerance)
  134. {
  135. PhysicsVector diff = this - v;
  136. float d = diff.length();
  137. if (d <= tolerance)
  138. return true;
  139. return false;
  140. }
  141. }
  142. }