From 2d5f5e2b32c5b8cc5bf6492964e06724b8935fd1 Mon Sep 17 00:00:00 2001 From: dan miller Date: Sun, 7 Oct 2007 14:40:02 +0000 Subject: applied Darok"s BulletXPlugin changes 003.patch --- libraries/ModifiedBulletX/ModifiedBulletX.suo | Bin 138240 -> 138240 bytes .../CollisionShapes/HeightfieldTerrainShape.cs | 360 +++++++++++++++++++++ .../Modified.XnaDevRu.BulletX.csproj | 1 + .../ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs | 11 +- .../MonoXnaCompactMaths/Quaternion.cs | 91 +++++- 5 files changed, 458 insertions(+), 5 deletions(-) create mode 100644 libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs (limited to 'libraries') diff --git a/libraries/ModifiedBulletX/ModifiedBulletX.suo b/libraries/ModifiedBulletX/ModifiedBulletX.suo index 4f0acd9..6244108 100644 Binary files a/libraries/ModifiedBulletX/ModifiedBulletX.suo and b/libraries/ModifiedBulletX/ModifiedBulletX.suo differ diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs new file mode 100644 index 0000000..0f30f1f --- /dev/null +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs @@ -0,0 +1,360 @@ +/* + * WARNING!: this class is not in the original BulletX + * By the way it's based on the Bullet btHeightfieldTerrainShape: + * http://www.continuousphysics.com/Bullet/BulletFull/classbtHeightfieldTerrainShape.html + ***************************************************************************************** + * 3RD PARTY LICENSE. The next it's the original 3rd party lincense of Bullet: + * ---------------------------------------------------------------------------- +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + * ------------------------------------------------------------------------------ +*/ +using System; +using System.Collections.Generic; +using System.Text; +using MonoXnaCompactMaths; + +namespace XnaDevRu.BulletX +{ + public class HeightfieldTerrainShape : ConcaveShape + { + private Vector3 _localAabbMin; + private Vector3 _localAabbMax; + private Vector3 _localScaling = new Vector3(1f,1f,1f); + private int _width; + private int _length; + private float[] _heightfieldData; + private float _maxHeight; + private int _upAxis; + private bool _useFloatData; + private bool _flipQuadEdges; + private bool _useDiamondSubdivision = false; + private float _defaultCollisionMargin = 0.6f; + + public HeightfieldTerrainShape(int width, int length, float[] heightfieldData, float maxHeight, + int upAxis, bool useFloatData, bool flipQuadEdges) + { + _width = width; + _length = length; + _heightfieldData = heightfieldData; + _maxHeight = maxHeight; + _upAxis = upAxis; + _useFloatData = useFloatData; + _flipQuadEdges = flipQuadEdges; + this.Margin = _defaultCollisionMargin; + + float quantizationMargin = 1f; + + //enlarge the AABB to avoid division by zero when initializing the quantization value + Vector3 clampValue = new Vector3(quantizationMargin, quantizationMargin, quantizationMargin); + Vector3 halfExtents = new Vector3(0, 0, 0); + + switch (_upAxis) + { + case 0: + halfExtents.X = _maxHeight; + halfExtents.Y = _width; + halfExtents.Z = _length; + break; + case 1: + halfExtents.X = _width; + halfExtents.Y = _maxHeight; + halfExtents.Z = _length; + break; + case 2: + halfExtents.X = _width; + halfExtents.Y = _length; + halfExtents.Z = _maxHeight; + break; + default: + //need to get valid _upAxis + //btAssert(0); + throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis"); + } + + halfExtents *= 0.5f; + + _localAabbMin = -halfExtents - clampValue; + _localAabbMax = halfExtents + clampValue; + //Vector3 aabbSize = new Vector3(); + //aabbSize = m_localAabbMax - m_localAabbMin; + + } + + protected Vector3 LocalAabbMin + { get { return _localAabbMin; } set { _localAabbMin = value; } } + protected Vector3 LocalAabbMax + { get { return _localAabbMax; } set { _localAabbMax = value; } } + public override string Name + { + get + { + return "HeightfieldTerrain"; + } + } + public override Vector3 LocalScaling + { + get + { + return _localScaling; + } + set + { + _localScaling = value; + } + } + public override float Margin + { + get + { + return base.Margin; + } + set + { + base.Margin = value; + } + } + public override BroadphaseNativeTypes ShapeType + { + get { return BroadphaseNativeTypes.Terrain; } + } + public Vector3 HalfExtents + { + get + { + Vector3 halfExtents = new Vector3(); + switch (_upAxis) + { + case 0: + halfExtents.X = 2f;//_maxHeight; + halfExtents.Y = _width; + halfExtents.Z = _length; + break; + case 1: + halfExtents.X = _width; + halfExtents.Y = 2f;// _maxHeight; + halfExtents.Z = _length; + break; + case 2: + halfExtents.X = _width; + halfExtents.Y = _length; + halfExtents.Z = 2f;// _maxHeight; + break; + default: + //need to get valid m_upAxis + //btAssert(0); + throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis"); + //break; + } + halfExtents *= 0.5f; + return halfExtents; + } + } + + public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) + { + //(void)callback; + //(void)aabbMax; + //(void)aabbMin; + + //quantize the aabbMin and aabbMax, and adjust the start/end ranges + + int[] quantizedAabbMin = new int[3]; + int[] quantizedAabbMax = new int[3]; + + Vector3 localAabbMin = aabbMin * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z ); + Vector3 localAabbMax = aabbMax * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z); + + quantizeWithClamp(ref quantizedAabbMin, localAabbMin); + quantizeWithClamp(ref quantizedAabbMax, localAabbMax); + + + + int startX=0; + int endX=_width-1; + int startJ=0; + int endJ=_length-1; + + switch(_upAxis) + { + case 0: + quantizedAabbMin[1]+=_width/2-1; + quantizedAabbMax[1]+=_width/2+1; + quantizedAabbMin[2]+=_length/2-1; + quantizedAabbMax[2]+=_length/2+1; + + if (quantizedAabbMin[1]>startX) + startX = quantizedAabbMin[1]; + if (quantizedAabbMax[1]startJ) + startJ = quantizedAabbMin[2]; + if (quantizedAabbMax[2]startX) + startX = quantizedAabbMin[0]; + if (quantizedAabbMax[0]startJ) + startJ = quantizedAabbMin[2]; + if (quantizedAabbMax[2]startX) + startX = quantizedAabbMin[0]; + if (quantizedAabbMax[0]startJ) + startJ = quantizedAabbMin[1]; + if (quantizedAabbMax[1] 0))) + { + //first triangle + getVertex(x,j,ref vertices[0]); + getVertex(x+1,j,ref vertices[1]); + getVertex(x+1,j+1,ref vertices[2]); + //callback->processTriangle(vertices,x,j); + callback.ProcessTriangle(vertices,x,j); + + //second triangle + getVertex(x,j,ref vertices[0]); + getVertex(x+1,j+1,ref vertices[1]); + getVertex(x,j+1,ref vertices[2]); + //callback->processTriangle(vertices,x,j); + callback.ProcessTriangle(vertices, x, j); + } + else + { + //first triangle + getVertex(x,j,ref vertices[0]); + getVertex(x,j+1,ref vertices[1]); + getVertex(x+1,j,ref vertices[2]); + //callback->processTriangle(vertices,x,j); + callback.ProcessTriangle(vertices,x,j); + + //second triangle + getVertex(x+1,j,ref vertices[0]); + getVertex(x,j+1,ref vertices[1]); + getVertex(x+1,j+1,ref vertices[2]); + //callback->processTriangle(vertices,x,j); + callback.ProcessTriangle(vertices,x,j); + } + } + } + } + public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax) + { + //aabbMin = new Vector3(-1e30f, -1e30f, -1e30f); + //aabbMax = new Vector3(1e30f, 1e30f, 1e30f); + + Vector3 halfExtents = (_localAabbMax - _localAabbMin) * _localScaling * 0.5f; + + Vector3 center = t.Translation; + Vector3 extent = new Vector3(Math.Abs(halfExtents.X), Math.Abs(halfExtents.Y), Math.Abs(halfExtents.Z)); + extent += new Vector3(Margin, Margin, Margin); + + aabbMin = center - extent; + aabbMax = center + extent; + } + public override void CalculateLocalInertia(float mass, out Vector3 inertia) + { + //moving concave objects not supported + inertia = new Vector3(); + } + public float getHeightFieldValue(int x,int y) + { + float val = 0f; + if (_useFloatData) + { + val = _heightfieldData[(y * _width) + x]; + } + else + { + //assume unsigned short int + int heightFieldValue = (int)_heightfieldData[(y * _width) + x]; + val = heightFieldValue * _maxHeight/65535f; + } + return val; + } + public void getVertex(int x,int y,ref Vector3 vertex) + { + if (x < 0) x = 0; + if (y < 0) y = 0; + if (x >= _width) x = _width - 1; + if (y >= _length) y = _length - 1; + float height = getHeightFieldValue(x,y); + switch(_upAxis) + { + case 0: + vertex.X = height; + vertex.Y = (- _width/2 ) + x; + vertex.Z = (- _length/2 ) + y; + break; + case 1: + vertex.X = (- _width/2 ) + x; + vertex.Y = height; + vertex.Z = (- _length/2 ) + y; + break; + case 2: + vertex.X = (- _width/2 ) + x; + vertex.Y = (- _length/2 ) + y; + vertex.Z = height; + break; + default: + //need to get valid m_upAxis + throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis"); + //break; + } + vertex *= _localScaling; + } + public void quantizeWithClamp(ref int[] _out,Vector3 point) + { + Vector3 clampedPoint = point; + MathHelper.SetMax(ref clampedPoint,_localAabbMin); + MathHelper.SetMin(ref clampedPoint, _localAabbMax); + Vector3 v = clampedPoint; + + _out[0] = (int)(v.X); + _out[1] = (int)(v.Y); + _out[2] = (int)(v.Z); + //correct for + } + } +} diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj index 06b98bc..3aead70 100644 --- a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj +++ b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj @@ -78,6 +78,7 @@ + diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs index 22842ca..9b88ab3 100644 --- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs @@ -573,7 +573,11 @@ namespace MonoXnaCompactMaths public static Matrix operator /(Matrix matrix1, float divider) { - throw new NotImplementedException(); + return new Matrix( + matrix1.M11 / divider, matrix1.M12 / divider, matrix1.M13 / divider, matrix1.M14 / divider, + matrix1.M21 / divider, matrix1.M22 / divider, matrix1.M23 / divider, matrix1.M24 / divider, + matrix1.M31 / divider, matrix1.M32 / divider, matrix1.M33 / divider, matrix1.M34 / divider, + matrix1.M41 / divider, matrix1.M42 / divider, matrix1.M43 / divider, matrix1.M44 / divider); } @@ -658,7 +662,10 @@ namespace MonoXnaCompactMaths public override string ToString() { - throw new NotImplementedException(); + return "[(" + this.M11 + ", " + this.M12 + ", " + this.M13 + ", " + this.M14 + ")\n (" + + this.M21 + ", " + this.M22 + ", " + this.M23 + ", " + this.M24 + ")\n (" + + this.M31 + ", " + this.M32 + ", " + this.M33 + ", " + this.M34 + ")\n (" + + this.M41 + ", " + this.M42 + ", " + this.M43 + ", " + this.M44 + ")]"; } diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs index b4f1873..b6d9d34 100644 --- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs +++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs @@ -90,7 +90,91 @@ namespace MonoXnaCompactMaths public static Quaternion CreateFromRotationMatrix(Matrix matrix) { - throw new NotImplementedException(); + float Omega2 = matrix.M44; + if (!isAprox(Omega2, 1f)) + { + //"Normalize" the Rotation matrix. Norma = M44 = Omega2 + matrix = matrix / Omega2; + } + //Deducted from: public static Matrix CreateFromQuaternion(Quaternion quaternion) + float lambda1pos, lambda2pos, lambda3pos, lambda1neg, lambda2neg, lambda3neg; + lambda1pos = (1f - matrix.M11 + matrix.M23 + matrix.M32) / 2f; + lambda2pos = (1f - matrix.M22 + matrix.M13 + matrix.M31) / 2f; + lambda3pos = (1f - matrix.M33 + matrix.M12 + matrix.M21) / 2f; + lambda1neg = (1f - matrix.M11 - matrix.M23 - matrix.M32) / 2f; + lambda2neg = (1f - matrix.M22 - matrix.M13 - matrix.M31) / 2f; + lambda3neg = (1f - matrix.M33 - matrix.M12 - matrix.M21) / 2f; + + //lambadIS = (qJ + s*qK)^2 + //q0 = w | q1 = x | q2 = y, q3 = z + //Every value of qI (I=1,2,3) has 4 possible values cause the sqrt + float[] x = new float[4]; float[] y = new float[4]; float[] z = new float[4]; + float[] sig1 = {1f, 1f, -1f, -1f}; + float[] sig2 = {1f, -1f, 1f, -1f}; + for (int i = 0; i < 4; i++) + { + x[i] = (sig1[i] * (float)Math.Sqrt(lambda1pos) + sig2[i] * (float)Math.Sqrt(lambda1neg)) / 2f; + y[i] = (sig1[i] * (float)Math.Sqrt(lambda2pos) + sig2[i] * (float)Math.Sqrt(lambda2neg)) / 2f; + z[i] = (sig1[i] * (float)Math.Sqrt(lambda3pos) + sig2[i] * (float)Math.Sqrt(lambda3neg)) / 2f; + } + + //Only a set of x, y, z are the corrects values. So it requires testing + int li_i=0, li_j=0, li_k=0; + bool lb_testL1P, lb_testL2P, lb_testL3P, lb_testL1N, lb_testL2N, lb_testL3N; + bool lb_superLambda = false; + while((li_i<4)&&(!lb_superLambda)) + { + while ((li_j < 4) && (!lb_superLambda)) + { + while ((li_k < 4) && (!lb_superLambda)) + { + lb_testL1P = isAprox((float)( + Math.Pow((double)(y[li_j] + z[li_k]), 2.0)), lambda1pos); + lb_testL2P = isAprox((float)( + Math.Pow((double)(x[li_i] + z[li_k]), 2.0)), lambda2pos); + lb_testL3P = isAprox((float)( + Math.Pow((double)(x[li_i] + y[li_j]), 2.0)), lambda3pos); + lb_testL1N = isAprox((float)( + Math.Pow((double)(y[li_j] - z[li_k]), 2.0)), lambda1neg); + lb_testL2N = isAprox((float)( + Math.Pow((double)(x[li_i] - z[li_k]), 2.0)), lambda2neg); + lb_testL3N = isAprox((float)( + Math.Pow((double)(x[li_i] - y[li_j]), 2.0)), lambda3neg); + + lb_superLambda = (lb_testL1P && lb_testL2P && lb_testL3P + && lb_testL1N && lb_testL2N && lb_testL3N); + + if (!lb_superLambda) li_k++; + } + if (!lb_superLambda) li_j++; + } + if (!lb_superLambda) li_i++; + } + + Quaternion q = new Quaternion(); + + if (lb_superLambda) + { + q.X = x[li_i]; q.Y = y[li_j]; q.Z = z[li_k]; + q.W = (matrix.M12 - 2f * q.X * q.Y) / (2f * q.Z); + + if (!isAprox(Omega2, 1f)) + { + if (Omega2 < 0) throw new Exception("Quaternion.CreateFromRotationMatrix: Omega2 is negative!"); + q = q * (float)Math.Sqrt(Omega2);//2 possibles values (+/-). For now only 1. + } + } + else + { + q = Quaternion.identity; + } + + return q; + } + private static float floatError = 0.000001f; + private static bool isAprox(float test, float realValue) + { + return (((realValue * (1f - floatError)) <= test) && (test <= (realValue * (1f + floatError)))); } @@ -317,7 +401,8 @@ namespace MonoXnaCompactMaths public static Quaternion operator *(Quaternion quaternion1, float scaleFactor) { - throw new NotImplementedException(); + return new Quaternion(quaternion1.X / scaleFactor, quaternion1.Y / scaleFactor, + quaternion1.Z / scaleFactor, quaternion1.W / scaleFactor); } @@ -335,7 +420,7 @@ namespace MonoXnaCompactMaths public override string ToString() { - throw new NotImplementedException(); + return "(" + this.X + ", " + this.Y + ", " + this.Z + ", " + this.W + ")"; } private static void Conjugate(ref Quaternion quaternion, out Quaternion result) -- cgit v1.1