public static IndexedMatrix CreateLookAt(IndexedVector3 cameraPosition, IndexedVector3 cameraTarget, IndexedVector3 cameraUpVector) { IndexedVector3 vector3_1 = IndexedVector3.Normalize(cameraPosition - cameraTarget); IndexedVector3 vector3_2 = IndexedVector3.Normalize(IndexedVector3.Cross(cameraUpVector, vector3_1)); IndexedVector3 vector1 = IndexedVector3.Cross(vector3_1, vector3_2); IndexedMatrix matrix = IndexedMatrix.Identity; //matrix._basis = new IndexedBasisMatrix(vector3_2.X, vector1.X, vector3_1.X, vector3_2.Y, vector1.Y, vector3_1.Y, vector3_2.Z, vector1.Z, vector3_1.Z).Transpose(); matrix._basis = new IndexedBasisMatrix(ref vector3_2, ref vector1, ref vector3_1); matrix._origin = new IndexedVector3(-IndexedVector3.Dot(vector3_2, cameraPosition), -IndexedVector3.Dot(vector1, cameraPosition), -IndexedVector3.Dot(vector3_1, cameraPosition)); return(matrix); }
public void CalcNormal(out IndexedVector3 normal) { normal = IndexedVector3.Cross(m_vertices1[1]-m_vertices1[0],m_vertices1[2]-m_vertices1[0]); normal.Normalize(); }
public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex) { //skip self-collisions if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex)) { return; } //skip duplicates (disabled for now) //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex)) // return; //search for shared vertices and edges int numshared = 0; int[] sharedVertsA = new int[] { -1, -1, -1 }; int[] sharedVertsB = new int[] { -1, -1, -1 }; ///skip degenerate triangles float crossBSqr = IndexedVector3.Cross((triangle[1] - triangle[0]), (triangle[2] - triangle[0])).LengthSquared(); if (crossBSqr < m_triangleInfoMap.m_equalVertexThreshold) { return; } float crossASqr = IndexedVector3.Cross((m_triangleVerticesA[1] - m_triangleVerticesA[0]), (m_triangleVerticesA[2] - m_triangleVerticesA[0])).LengthSquared(); ///skip degenerate triangles if (crossASqr < m_triangleInfoMap.m_equalVertexThreshold) { return; } #if false printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n", m_triangleVerticesA[0].GetX(),m_triangleVerticesA[0].GetY(),m_triangleVerticesA[0].GetZ(), m_triangleVerticesA[1].GetX(),m_triangleVerticesA[1].GetY(),m_triangleVerticesA[1].GetZ(), m_triangleVerticesA[2].GetX(),m_triangleVerticesA[2].GetY(),m_triangleVerticesA[2].GetZ()); printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex); printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n", triangle[0].GetX(),triangle[0].GetY(),triangle[0].GetZ(), triangle[1].GetX(),triangle[1].GetY(),triangle[1].GetZ(), triangle[2].GetX(),triangle[2].GetY(),triangle[2].GetZ()); #endif for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { if ((m_triangleVerticesA[i] - triangle[j]).LengthSquared() < m_triangleInfoMap.m_equalVertexThreshold) { sharedVertsA[numshared] = i; sharedVertsB[numshared] = j; numshared++; ///degenerate case if (numshared >= 3) { return; } } } ///degenerate case if (numshared >= 3) { return; } } switch (numshared) { case 0: { break; } case 1: { //shared vertex break; } case 2: { //shared edge //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2) { sharedVertsA[0] = 2; sharedVertsA[1] = 0; int tmp = sharedVertsB[1]; sharedVertsB[1] = sharedVertsB[0]; sharedVertsB[0] = tmp; } int hash = GetHash(m_partIdA, m_triangleIndexA); TriangleInfo info = null; if (m_triangleInfoMap.ContainsKey(hash)) { info = m_triangleInfoMap[hash]; } else { info = new TriangleInfo(); m_triangleInfoMap[hash] = info; } int sumvertsA = sharedVertsA[0] + sharedVertsA[1]; int otherIndexA = 3 - sumvertsA; IndexedVector3 edge = new IndexedVector3(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]); TriangleShape tA = new TriangleShape(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]); int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]); TriangleShape tB = new TriangleShape(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]); //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); IndexedVector3 normalA; IndexedVector3 normalB; tA.CalcNormal(out normalA); tB.CalcNormal(out normalB); edge.Normalize(); IndexedVector3 edgeCrossA = IndexedVector3.Normalize(IndexedVector3.Cross(edge, normalA)); { IndexedVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]]; if (IndexedVector3.Dot(edgeCrossA, tmp) < 0) { edgeCrossA *= -1; } } IndexedVector3 edgeCrossB = IndexedVector3.Cross(edge, normalB).Normalized(); { IndexedVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]]; if (IndexedVector3.Dot(edgeCrossB, tmp) < 0) { edgeCrossB *= -1; } } float angle2 = 0; float ang4 = 0.0f; IndexedVector3 calculatedEdge = IndexedVector3.Cross(edgeCrossA, edgeCrossB); float len2 = calculatedEdge.LengthSquared(); float correctedAngle = 0f; IndexedVector3 calculatedNormalB = normalA; bool isConvex = false; if (len2 < m_triangleInfoMap.m_planarEpsilon) { angle2 = 0.0f; ang4 = 0.0f; } else { calculatedEdge.Normalize(); IndexedVector3 calculatedNormalA = IndexedVector3.Cross(calculatedEdge, edgeCrossA); calculatedNormalA.Normalize(); angle2 = GetAngle(ref calculatedNormalA, ref edgeCrossA, ref edgeCrossB); ang4 = MathUtil.SIMD_PI - angle2; float dotA = IndexedVector3.Dot(normalA, edgeCrossB); ///@todo: check if we need some epsilon, due to floating point imprecision isConvex = (dotA < 0f); correctedAngle = isConvex ? ang4 : -ang4; IndexedQuaternion orn2 = new IndexedQuaternion(calculatedEdge, -correctedAngle); IndexedMatrix rotateMatrix = IndexedMatrix.CreateFromQuaternion(orn2); calculatedNormalB = new IndexedBasisMatrix(orn2) * normalA; } //alternatively use //IndexedVector3 calculatedNormalB2 = quatRotate(orn,normalA); switch (sumvertsA) { case 1: { IndexedVector3 edge1 = m_triangleVerticesA[0] - m_triangleVerticesA[1]; IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA); float bla = IndexedVector3.Dot(computedNormalB, normalB); if (bla < 0) { computedNormalB *= -1; info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB; } #if DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).Length() > 0.0001f) { System.Console.WriteLine("warning: normals not identical"); } #endif//DEBUG_INTERNAL_EDGE info.m_edgeV0V1Angle = -correctedAngle; if (isConvex) { info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_CONVEX; } break; } case 2: { IndexedVector3 edge1 = m_triangleVerticesA[2] - m_triangleVerticesA[0]; IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA); if (IndexedVector3.Dot(computedNormalB, normalB) < 0) { computedNormalB *= -1; info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB; } #if DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).Length() > 0.0001) { System.Console.WriteLine("warning: normals not identical"); } #endif //DEBUG_INTERNAL_EDGE info.m_edgeV2V0Angle = -correctedAngle; if (isConvex) info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_CONVEX; break; } case 3: { IndexedVector3 edge1 = m_triangleVerticesA[1] - m_triangleVerticesA[2]; IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle); IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA); if (IndexedVector3.Dot(computedNormalB, normalB) < 0) { info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB; computedNormalB *= -1; } #if DEBUG_INTERNAL_EDGE if ((computedNormalB - normalB).Length() > 0.0001) { System.Console.WriteLine("warning: normals not identical"); } #endif //DEBUG_INTERNAL_EDGE info.m_edgeV1V2Angle = -correctedAngle; if (isConvex) { info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_CONVEX; } break; } } break; } default: { // printf("warning: duplicate triangle\n"); break; } } }
public override void InitializeDemo() { //string filename = @"E:\users\man\bullet\xna-concaveray-output-1.txt"; //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read); //BulletGlobals.g_streamWriter = new StreamWriter(filestream); m_cameraDistance = 400f; m_farClip = 1500f; m_perspective = IndexedMatrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians(40.0f), m_aspect, m_nearClip, m_farClip); m_animatedMesh = true; base.InitializeDemo(); int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1); gVertices = new ObjectArray<IndexedVector3>(totalVerts); int indicesTotal = totalTriangles * 3; gIndices = new ObjectArray<int>(indicesTotal); BulletGlobals.gContactAddedCallback = null; SetVertexPositions(waveheight,0f); int vertStride = 1; int indexStride = 3; int index=0; //for (int i=0;i<NUM_VERTS_X-1;i++) //{ // for (int j=0;j<NUM_VERTS_Y-1;j++) // { // gIndices[index++] = j * NUM_VERTS_X + i; // gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1; // gIndices[index++] = j * NUM_VERTS_X + i + 1; // gIndices[index++] = j * NUM_VERTS_X + i; // gIndices[index++] = (j + 1) * NUM_VERTS_X + i; // gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1; // } //} for (int i = 0; i < NUM_VERTS_X - 1; i++) { for (int j = 0; j < NUM_VERTS_Y - 1; j++) { gIndices[index++] = j * NUM_VERTS_X + i; gIndices[index++] = j * NUM_VERTS_X + i + 1; gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1; gIndices[index++] = j * NUM_VERTS_X + i; gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1; gIndices[index++] = (j + 1) * NUM_VERTS_X + i; } } TriangleIndexVertexArray indexVertexArrays = new TriangleIndexVertexArray(totalTriangles, gIndices,indexStride,totalVerts,gVertices,vertStride); bool useQuantizedAabbCompression = true; float minX = NUM_VERTS_X * TRIANGLE_SIZE * 0.5f; float minZ = NUM_VERTS_Y * TRIANGLE_SIZE * 0.5f; //OptimizedBvh bvh = new OptimizedBvh(); IndexedVector3 aabbMin = new IndexedVector3(-minX,-5,-minZ); IndexedVector3 aabbMax = new IndexedVector3(minX,5,minZ); m_trimeshShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression, ref aabbMin, ref aabbMax, true); //m_trimeshShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression,true); IndexedVector3 scaling = IndexedVector3.One; CollisionShape groundShape = m_trimeshShape; //groundShape = new TriangleMeshShape(indexVertexArrays); //groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f); IndexedVector3 up = new IndexedVector3(0.4f,1,0); up.Normalize(); //groundShape = new StaticPlaneShape(up, 0f); //groundShape = new BoxShape(new IndexedVector3(1000, 10, 1000)); m_collisionConfiguration = new DefaultCollisionConfiguration(); m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); IndexedVector3 worldMin = aabbMin; IndexedVector3 worldMax = aabbMax; m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false); m_constraintSolver = new SequentialImpulseConstraintSolver(); m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration); float mass = 0f; IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(2,-2,0)); startTransform = IndexedMatrix.Identity; staticBody = LocalCreateRigidBody(mass, ref startTransform,groundShape); staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT);//STATIC_OBJECT); //enable custom material callback staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK); //m_raycastBar = new btRaycastBar (m_debugDraw,4000.0f, 0.0f,-1000,30); m_raycastBar = new btRaycastBar(m_debugDraw, 300.0f, 0.0f, -1000, 200,worldMin.X,worldMax.X); m_raycastBar.min_x = -100; m_raycastBar.max_x = -100; ClientResetScene(); }
public eStatus Evaluate(GJK gjk,ref IndexedVector3 guess) { sSimplex simplex=gjk.m_simplex; if((simplex.rank>1)&&gjk.EncloseOrigin()) { /* Clean up */ while(m_hull.Count > 0) { sFace f = m_hull[0]; Remove(m_hull,f); Append(m_stock,f); } m_status = eStatus.Valid; m_nextsv = 0; /* Orient simplex */ if(GJK.Det( simplex.c[0].w-simplex.c[3].w, simplex.c[1].w-simplex.c[3].w, simplex.c[2].w-simplex.c[3].w)<0) { SwapSv(simplex.c,0,1); SwapFloat(simplex.p,0,1); } /* Build initial hull */ tetra[0] = NewFace(simplex.c[0], simplex.c[1], simplex.c[2], true); tetra[1] = NewFace(simplex.c[1], simplex.c[0], simplex.c[3], true); tetra[2] = NewFace(simplex.c[2], simplex.c[1], simplex.c[3], true); tetra[3] = NewFace(simplex.c[0], simplex.c[2], simplex.c[3], true); if(m_hull.Count==4) { sFace best=FindBest(); sFace outer = best; uint pass=0; uint iterations=0; Bind(tetra[0],0,tetra[1],0); Bind(tetra[0],1,tetra[2],0); Bind(tetra[0],2,tetra[3],0); Bind(tetra[1],1,tetra[3],2); Bind(tetra[1],2,tetra[2],1); Bind(tetra[2],2,tetra[3],1); m_status=eStatus.Valid; for (; iterations < GjkEpaSolver2.EPA_MAX_ITERATIONS; ++iterations) { if (m_nextsv < GjkEpaSolver2.EPA_MAX_VERTICES) { sHorizon horizon = new sHorizon() ; sSV w = m_sv_store[m_nextsv++]; bool valid = true; best.pass = (uint)(++pass); gjk.GetSupport(ref best.n,ref w); float wdist=IndexedVector3.Dot(ref best.n,ref w.w)-best.d; if (wdist > GjkEpaSolver2.EPA_ACCURACY) { for(int j=0;(j<3)&&valid;++j) { valid&=Expand( pass,w, best.f[j],best.e[j], ref horizon); } if(valid&&(horizon.nf>=3)) { Bind(horizon.cf,1,horizon.ff,2); Remove(m_hull,best); Append(m_stock,best); best=FindBest(); if (best.p >= outer.p) { outer = best; } } else { m_status=eStatus.InvalidHull; break; } } else { m_status=eStatus.AccuraryReached; break; } } else { m_status=eStatus.OutOfVertices; break; } } IndexedVector3 projection=outer.n*outer.d; m_normal = outer.n; m_depth = outer.d; m_result.rank = 3; m_result.c[0] = outer.c[0]; m_result.c[1] = outer.c[1]; m_result.c[2] = outer.c[2]; m_result.p[0] = IndexedVector3.Cross( outer.c[1].w-projection, outer.c[2].w-projection).Length(); m_result.p[1] = IndexedVector3.Cross(outer.c[2].w - projection, outer.c[0].w-projection).Length(); m_result.p[2] = IndexedVector3.Cross(outer.c[0].w - projection, outer.c[1].w-projection).Length(); float sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; m_result.p[0] /= sum; m_result.p[1] /= sum; m_result.p[2] /= sum; return(m_status); } } /* Fallback */ m_status = eStatus.FallBack; m_normal = -guess; float nl=m_normal.LengthSquared(); if(nl>0) { m_normal.Normalize(); } else { m_normal = new IndexedVector3(1,0,0); } m_depth = 0; m_result.rank=1; m_result.c[0]=simplex.c[0]; m_result.p[0]=1; return(m_status); }
//---------------------------------------------------------------------------------------------- public DemoApplication() { m_dynamicsWorld = null; m_pickConstraint = null; m_shootBoxShape = null; m_cameraDistance = 30f; m_pitch =(20f/360f)*MathUtil.SIMD_2_PI; m_yaw = 0f; m_cameraPosition = IndexedVector3.Zero; m_cameraTargetPosition = IndexedVector3.Zero; m_scaleBottom = 0.5f; m_scaleFactor = 2f; m_cameraUp = new IndexedVector3(0, 1, 0); m_forwardAxis = 2; m_glutScreenWidth = 0; m_glutScreenHeight = 0; m_ShootBoxInitialSpeed = 40f; m_stepping = true; m_singleStep = false; m_idle = false; m_enableshadows = true; m_lightPosition = new IndexedVector3(5, 5, 5); //m_lightDirection = IndexedVector3.Down; m_lightDirection = new IndexedVector3(.5f, -.5f, .5f); m_lightDirection.Normalize(); //#ifndef BT_NO_PROFILE // m_profileIterator = CProfileManager::Get_Iterator(); //#endif //BT_NO_PROFILE Content.RootDirectory = "Content"; m_graphics = new GraphicsDeviceManager(this); m_graphics.PreferredBackBufferWidth = 800; m_graphics.PreferredBackBufferHeight = 600; SetSize(m_graphics.PreferredBackBufferWidth,m_graphics.PreferredBackBufferHeight); m_nearClip = 1f; m_farClip = 1000f; m_aspect = m_glutScreenWidth / m_glutScreenHeight; m_perspective = IndexedMatrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians(40.0f), m_aspect, m_nearClip, m_farClip); }
//---------------------------------------------------------------------------------------------- public IndexedVector3 GetRayTo(int x, int y) { float fov = MathHelper.ToRadians(40.0f); IndexedVector3 rayFrom = new IndexedVector3(GetCameraPosition()); IndexedVector3 rayForward = new IndexedVector3((GetCameraTargetPosition() - GetCameraPosition())); rayForward.Normalize(); float farPlane = 10000f; rayForward *= farPlane; IndexedVector3 vertical = new IndexedVector3(m_cameraUp); IndexedVector3 hor = IndexedVector3.Cross(rayForward, vertical); hor.Normalize(); vertical = IndexedVector3.Cross(hor, rayForward); vertical.Normalize(); float tanfov = (float)Math.Tan(0.5f * fov); hor *= 2f * farPlane * tanfov; vertical *= 2f * farPlane * tanfov; float aspect = 1f; if (m_glutScreenWidth > m_glutScreenHeight) { aspect = m_glutScreenWidth / (float)m_glutScreenHeight; hor *= aspect; } else { aspect = m_glutScreenHeight / (float)m_glutScreenWidth; vertical *= aspect; } IndexedVector3 rayToCenter = rayFrom + rayForward; IndexedVector3 dHor = hor * 1f / (float)m_glutScreenWidth; IndexedVector3 dVert = vertical * 1f / (float)m_glutScreenHeight; IndexedVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; rayTo += x * dHor; rayTo -= y * dVert; return rayTo; }
public void ShootTrimesh(IndexedVector3 startPosition, IndexedVector3 destination) { if (m_dynamicsWorld != null) { float mass = 4.0f; IndexedMatrix startTransform = IndexedMatrix.Identity; startTransform._origin = new IndexedVector3(startPosition); RigidBody body = LocalCreateRigidBody(mass, startTransform,m_trimeshShape2); IndexedVector3 linVel = new IndexedVector3(destination - startPosition); linVel.Normalize(); linVel*=m_ShootBoxInitialSpeed*0.25f; body.SetLinearVelocity(ref linVel); body.SetAngularVelocity(IndexedVector3.Zero); } }
public override void InitializeDemo() { m_cameraDistance = 10.0f; //string filename = @"e:\users\man\bullet\gimpact-demo-xna.txt"; //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read); //BulletGlobals.g_streamWriter = new StreamWriter(filestream); /// Init Bullet m_collisionConfiguration = new DefaultCollisionConfiguration(); m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); //btOverlappingPairCache* broadphase = new btSimpleBroadphase(); //m_broadphase = new btSimpleBroadphase(); int maxProxies = 1024; IndexedVector3 worldAabbMin = new IndexedVector3(-10000, -10000, -10000); IndexedVector3 worldAabbMax = new IndexedVector3(10000, 10000, 10000); m_broadphase = new AxisSweep3Internal(ref worldAabbMin, ref worldAabbMax, 0xfffe, 0xffff, 16384, null, false); //m_broadphase = new SimpleBroadphase(16384,null); m_constraintSolver = new SequentialImpulseConstraintSolver(); m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration); //create trimesh model and shape InitGImpactCollision(); /// Create Scene float mass = 0.0f; IndexedMatrix startTransform = IndexedMatrix.Identity; CollisionShape staticboxShape1 = new BoxShape(new IndexedVector3(200, 1, 200));//floor staticboxShape1.SetUserPointer("Floor"); CollisionShape staticboxShape2 = new BoxShape(new IndexedVector3(1, 50, 200));//left wall staticboxShape1.SetUserPointer("LeftWall"); CollisionShape staticboxShape3 = new BoxShape(new IndexedVector3(1, 50, 200));//right wall staticboxShape1.SetUserPointer("RightWall"); CollisionShape staticboxShape4 = new BoxShape(new IndexedVector3(200, 50, 1));//front wall staticboxShape1.SetUserPointer("FrontWall"); CollisionShape staticboxShape5 = new BoxShape(new IndexedVector3(200, 50, 1));//back wall staticboxShape1.SetUserPointer("BackWall"); CompoundShape staticScenario = new CompoundShape();//static scenario startTransform._origin = new IndexedVector3(0, 0, 0); staticScenario.AddChildShape(ref startTransform, staticboxShape1); startTransform._origin = new IndexedVector3(-200, 25, 0); staticScenario.AddChildShape(ref startTransform, staticboxShape2); startTransform._origin = new IndexedVector3(200, 25, 0); staticScenario.AddChildShape(ref startTransform, staticboxShape3); startTransform._origin = new IndexedVector3(0, 25, 200); staticScenario.AddChildShape(ref startTransform, staticboxShape4); startTransform._origin = new IndexedVector3(0, 25, -200); staticScenario.AddChildShape(ref startTransform, staticboxShape5); startTransform._origin = new IndexedVector3(0, 0, 0); //RigidBody staticBody = LocalCreateRigidBody(mass, startTransform, staticScenario); RigidBody staticBody = LocalCreateRigidBody(mass, startTransform, staticboxShape1); staticBody.SetCollisionFlags(staticBody.GetCollisionFlags()|CollisionFlags.CF_STATIC_OBJECT); //enable custom material callback staticBody.SetCollisionFlags(staticBody.GetCollisionFlags()|CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK); //static plane IndexedVector3 normal = new IndexedVector3(0.4f,1.5f,-0.4f); normal.Normalize(); CollisionShape staticplaneShape6 = new StaticPlaneShape(ref normal,0.0f);// A plane startTransform._origin = IndexedVector3.Zero; RigidBody staticBody2 = LocalCreateRigidBody(mass, startTransform, staticplaneShape6); staticBody2.SetCollisionFlags(staticBody2.GetCollisionFlags()|CollisionFlags.CF_STATIC_OBJECT); startTransform = IndexedMatrix.Identity; /// Create Dynamic Boxes { int numBoxes = 1; for (int i = 0; i < numBoxes; i++) { CollisionShape boxShape = new BoxShape(new IndexedVector3(1, 1, 1)); //CollisionShape mesh = new BvhTriangleMeshShape(m_indexVertexArrays2,true,true); startTransform._origin = new IndexedVector3(2 * i - (numBoxes-1), 2, -3); //startTransform._origin = new IndexedVector3(2 * i - 5, 10, -3); //LocalCreateRigidBody(1, startTransform, m_trimeshShape2); LocalCreateRigidBody(1, startTransform, boxShape); } } }
protected void ComputeTwistLimitInfo(ref IndexedQuaternion qTwist, // in out float twistAngle, out IndexedVector3 vTwistAxis) // all outs { IndexedQuaternion qMinTwist = qTwist; twistAngle = MathUtil.QuatAngle(ref qTwist); if (twistAngle > MathUtil.SIMD_PI) // long way around. flip quat and recalculate. { qMinTwist = -(qTwist); twistAngle = MathUtil.QuatAngle(ref qTwist); } if (twistAngle < 0f) { // this should never happen Debug.Assert(false); } vTwistAxis = new IndexedVector3(qMinTwist.X, qMinTwist.Y, qMinTwist.Z); if (twistAngle > MathUtil.SIMD_EPSILON) { vTwistAxis.Normalize(); } }
protected void AdjustSwingAxisToUseEllipseNormal(ref IndexedVector3 vSwingAxis) { // the swing axis is computed as the "twist-free" cone rotation, // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2). // so, if we're outside the limits, the closest way back inside the cone isn't // along the vector back to the center. better (and more stable) to use the ellipse normal. // convert swing axis to direction from center to surface of ellipse // (ie. rotate 2D vector by PI/2) float y = -vSwingAxis.Z; float z = vSwingAxis.Y; // do the math... if (Math.Abs(z) > MathUtil.SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. { // compute gradient/normal of ellipse surface at current "point" float grad = y / z; grad *= m_swingSpan2 / m_swingSpan1; // adjust y/z to represent normal at point (instead of vector to point) if (y > 0) { y = Math.Abs(grad * z); } else { y = -Math.Abs(grad * z); } // convert ellipse direction back to swing axis vSwingAxis.Z = -y; vSwingAxis.Y = z; vSwingAxis.Normalize(); } }
protected void ComputeConeLimitInfo(ref IndexedQuaternion qCone, // in ref float swingAngle, ref IndexedVector3 vSwingAxis, ref float swingLimit) // all outs { swingAngle = MathUtil.QuatAngle(ref qCone); if (swingAngle > MathUtil.SIMD_EPSILON) { vSwingAxis = new IndexedVector3(qCone.X, qCone.Y, qCone.Z); vSwingAxis.Normalize(); if (Math.Abs(vSwingAxis.X) > MathUtil.SIMD_EPSILON) { // non-zero twist?! this should never happen. Debug.Assert(false); } // Compute limit for given swing. tricky: // Given a swing axis, we're looking for the intersection with the bounding cone ellipse. // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.) // For starters, compute the direction from center to surface of ellipse. // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis. // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.) float xEllipse = vSwingAxis.Y; float yEllipse = -vSwingAxis.Z; // Now, we use the slope of the vector (using x/yEllipse) and find the length // of the line that intersects the ellipse: // x^2 y^2 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) // a^2 b^2 // Do the math and it should be clear. swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 if (Math.Abs(xEllipse) > MathUtil.SIMD_EPSILON) { float surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse); float norm = 1f / (m_swingSpan2 * m_swingSpan2); norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); float swingLimit2 = (1 + surfaceSlope2) / norm; swingLimit = (float)Math.Sqrt(swingLimit2); } // test! /*swingLimit = m_swingSpan2; if (fabs(vSwingAxis.z()) > SIMD_EPSILON) { float mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2; float sinphi = m_swingSpan2 / sqrt(mag_2); float phi = asin(sinphi); float theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z())); float alpha = 3.14159f - theta - phi; float sinalpha = sin(alpha); swingLimit = m_swingSpan1 * sinphi/sinalpha; }*/ } else if (swingAngle < 0) { // this should never happen! Debug.Assert(false); } }
public void CalcAngleInfo2(ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedBasisMatrix invInertiaWorldA, ref IndexedBasisMatrix invInertiaWorldB) { m_swingCorrection = 0; m_twistLimitSign = 0; m_solveTwistLimit = false; m_solveSwingLimit = false; // compute rotation of A wrt B (in constraint space) if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) { // it is assumed that setMotorTarget() was alredy called // and motor target m_qTarget is within constraint limits // TODO : split rotation to pure swing and pure twist // compute desired transforms in world IndexedMatrix trPose = IndexedMatrix.CreateFromQuaternion(m_qTarget); IndexedMatrix trA = transA * m_rbAFrame; IndexedMatrix trB = transB * m_rbBFrame; IndexedMatrix trDeltaAB = trB * trPose * trA.Inverse(); IndexedQuaternion qDeltaAB = trDeltaAB.GetRotation(); IndexedVector3 swingAxis = new IndexedVector3(qDeltaAB.X, qDeltaAB.Y, qDeltaAB.Z); float swingAxisLen2 = swingAxis.LengthSquared(); if (MathUtil.FuzzyZero(swingAxisLen2)) { return; } m_swingAxis = swingAxis; m_swingAxis.Normalize(); m_swingCorrection = MathUtil.QuatAngle(ref qDeltaAB); if (!MathUtil.FuzzyZero(m_swingCorrection)) { m_solveSwingLimit = true; } return; } { // compute rotation of A wrt B (in constraint space) // Not sure if these need order swapping as well? IndexedQuaternion qA = transA.GetRotation() * m_rbAFrame.GetRotation(); IndexedQuaternion qB = transB.GetRotation() * m_rbBFrame.GetRotation(); IndexedQuaternion qAB = MathUtil.QuaternionInverse(qB) * qA; // split rotation into cone and twist // (all this is done from B's perspective. Maybe I should be averaging axes...) IndexedVector3 vConeNoTwist = MathUtil.QuatRotate(ref qAB, ref vTwist); vConeNoTwist.Normalize(); IndexedQuaternion qABCone = MathUtil.ShortestArcQuat(ref vTwist, ref vConeNoTwist); qABCone.Normalize(); IndexedQuaternion qABTwist = MathUtil.QuaternionInverse(qABCone) * qAB; qABTwist.Normalize(); if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) { float swingAngle = 0f, swingLimit = 0f; IndexedVector3 swingAxis = IndexedVector3.Zero; ComputeConeLimitInfo(ref qABCone, ref swingAngle, ref swingAxis, ref swingLimit); if (swingAngle > swingLimit * m_limitSoftness) { m_solveSwingLimit = true; // compute limit ratio: 0->1, where // 0 == beginning of soft limit // 1 == hard/real limit m_swingLimitRatio = 1f; if (swingAngle < swingLimit && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON) { m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) / (swingLimit - (swingLimit * m_limitSoftness)); } // swing correction tries to get back to soft limit m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); // adjustment of swing axis (based on ellipse normal) AdjustSwingAxisToUseEllipseNormal(ref swingAxis); // Calculate necessary axis & factors m_swingAxis = MathUtil.QuatRotate(qB, -swingAxis); m_twistAxisA = IndexedVector3.Zero; m_kSwing = 1f / (ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldA) + ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldB)); } } else { // you haven't set any limits; // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) // anyway, we have either hinge or fixed joint IndexedVector3 ivA = transA._basis * m_rbAFrame._basis.GetColumn(0); IndexedVector3 jvA = transA._basis * m_rbAFrame._basis.GetColumn(1); IndexedVector3 kvA = transA._basis * m_rbAFrame._basis.GetColumn(2); IndexedVector3 ivB = transB._basis * m_rbBFrame._basis.GetColumn(0); IndexedVector3 target = IndexedVector3.Zero; float x = ivB.Dot(ref ivA); float y = ivB.Dot(ref jvA); float z = ivB.Dot(ref kvA); if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { // fixed. We'll need to add one more row to constraint if ((!MathUtil.FuzzyZero(y)) || (!(MathUtil.FuzzyZero(z)))) { m_solveSwingLimit = true; m_swingAxis = -ivB.Cross(ref ivA); } } else { if (m_swingSpan1 < m_fixThresh) { // hinge around Y axis if (!(MathUtil.FuzzyZero(y))) { m_solveSwingLimit = true; if (m_swingSpan2 >= m_fixThresh) { y = 0; float span2 = (float)Math.Atan2(z, x); if (span2 > m_swingSpan2) { x = (float)Math.Cos(m_swingSpan2); z = (float)Math.Sin(m_swingSpan2); } else if (span2 < -m_swingSpan2) { x = (float)Math.Cos(m_swingSpan2); z = -(float)Math.Sin(m_swingSpan2); } } } } else { // hinge around Z axis if (!MathUtil.FuzzyZero(z)) { m_solveSwingLimit = true; if (m_swingSpan1 >= m_fixThresh) { z = 0f; float span1 = (float)Math.Atan2(y, x); if (span1 > m_swingSpan1) { x = (float)Math.Cos(m_swingSpan1); y = (float)Math.Sin(m_swingSpan1); } else if (span1 < -m_swingSpan1) { x = (float)Math.Cos(m_swingSpan1); y = -(float)Math.Sin(m_swingSpan1); } } } } target.X = x * ivA.X + y * jvA.X + z * kvA.X; target.Y = x * ivA.Y + y * jvA.Y + z * kvA.Y; target.Z = x * ivA.Z + y * jvA.Z + z * kvA.Z; target.Normalize(); m_swingAxis = -(ivB.Cross(ref target)); m_swingCorrection = m_swingAxis.Length(); m_swingAxis.Normalize(); } } if (m_twistSpan >= 0f) { IndexedVector3 twistAxis; ComputeTwistLimitInfo(ref qABTwist, out m_twistAngle, out twistAxis); if (m_twistAngle > m_twistSpan * m_limitSoftness) { m_solveTwistLimit = true; m_twistLimitRatio = 1f; if (m_twistAngle < m_twistSpan && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON) { m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) / (m_twistSpan - m_twistSpan * m_limitSoftness); } // twist correction tries to get back to soft limit m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness); m_twistAxis = MathUtil.QuatRotate(qB, -twistAxis); m_kTwist = 1f / (ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldA) + ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldB)); } if (m_solveSwingLimit) { m_twistAxisA = MathUtil.QuatRotate(qA, -twistAxis); } } else { m_twistAngle = 0f; } } }
public void CalcAngleInfo() { m_swingCorrection = 0f; m_twistLimitSign = 0f; m_solveTwistLimit = false; m_solveSwingLimit = false; IndexedVector3 b1Axis1 = IndexedVector3.Zero, b1Axis2 = IndexedVector3.Zero, b1Axis3 = IndexedVector3.Zero; IndexedVector3 b2Axis1 = IndexedVector3.Zero, b2Axis2 = IndexedVector3.Zero; IndexedMatrix transA = GetRigidBodyA().GetCenterOfMassTransform(); IndexedMatrix transB = GetRigidBodyB().GetCenterOfMassTransform(); b1Axis1 = GetRigidBodyA().GetCenterOfMassTransform()._basis * m_rbAFrame._basis.GetColumn(0); b2Axis1 = GetRigidBodyB().GetCenterOfMassTransform()._basis * m_rbBFrame._basis.GetColumn(0); float swing1 = 0f, swing2 = 0f; float swx = 0f, swy = 0f; float thresh = 10f; float fact; // Get Frame into world space if (m_swingSpan1 >= 0.05f) { b1Axis2 = GetRigidBodyA().GetCenterOfMassTransform()._basis * m_rbAFrame._basis.GetColumn(1); swx = b2Axis1.Dot(ref b1Axis1); swy = b2Axis1.Dot(ref b1Axis2); swing1 = (float)Math.Atan2(swy, swx); fact = (swy * swy + swx * swx) * thresh * thresh; fact = fact / (fact + 1f); swing1 *= fact; } if (m_swingSpan2 >= 0.05f) { b1Axis3 = GetRigidBodyA().GetCenterOfMassTransform()._basis * m_rbAFrame._basis.GetColumn(2); swx = b2Axis1.Dot(ref b1Axis1); swy = b2Axis1.Dot(ref b1Axis3); swing2 = (float)Math.Atan2(swy, swx); fact = (swy * swy + swx * swx) * thresh * thresh; fact = fact / (fact + 1f); swing2 *= fact; } float RMaxAngle1Sq = 1.0f / (m_swingSpan1 * m_swingSpan1); float RMaxAngle2Sq = 1.0f / (m_swingSpan2 * m_swingSpan2); float EllipseAngle = Math.Abs(swing1 * swing1) * RMaxAngle1Sq + Math.Abs(swing2 * swing2) * RMaxAngle2Sq; if (EllipseAngle > 1.0f) { m_swingCorrection = EllipseAngle - 1.0f; m_solveSwingLimit = true; // Calculate necessary axis & factors m_swingAxis = b2Axis1.Cross(b1Axis2 * b2Axis1.Dot(ref b1Axis2) + b1Axis3 * b2Axis1.Dot(ref b1Axis3)); m_swingAxis.Normalize(); float swingAxisSign = (b2Axis1.Dot(ref b1Axis1) >= 0.0f) ? 1.0f : -1.0f; m_swingAxis *= swingAxisSign; } // Twist limits if (m_twistSpan >= 0f) { IndexedVector3 b2Axis2a = GetRigidBodyB().GetCenterOfMassTransform()._basis * m_rbBFrame._basis.GetColumn(1); IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref b2Axis1, ref b1Axis1); IndexedVector3 TwistRef = MathUtil.QuatRotate(ref rotationArc, ref b2Axis2a); float twist = (float)Math.Atan2(TwistRef.Dot(ref b1Axis3), TwistRef.Dot(ref b1Axis2)); m_twistAngle = twist; // float lockedFreeFactor = (m_twistSpan > float(0.05f)) ? m_limitSoftness : float(0.); float lockedFreeFactor = (m_twistSpan > 0.05f) ? 1.0f : 0f; if (twist <= -m_twistSpan * lockedFreeFactor) { m_twistCorrection = -(twist + m_twistSpan); m_solveTwistLimit = true; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.Normalize(); m_twistAxis *= -1.0f; } else if (twist > m_twistSpan * lockedFreeFactor) { m_twistCorrection = (twist - m_twistSpan); m_solveTwistLimit = true; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis.Normalize(); } } }
//----------------------------------------------------------------------------------------------- public override void InitializeDemo() { //string filename = @"C:\users\man\bullett\xna-concave-output.txt"; //FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read); //BulletGlobals.g_streamWriter = new StreamWriter(filestream); m_animatedMesh = true; base.InitializeDemo(); int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1); gVertices = new ObjectArray<IndexedVector3>(totalVerts); int indicesTotal = totalTriangles * 3; gIndices = new ObjectArray<int>(indicesTotal); //BulletGlobals.gContactAddedCallback = new CustomMaterialCombinerCallback(); SetVertexPositions(waveheight,0f); int vertStride = 1; int indexStride = 3; int index=0; for (int i=0;i<NUM_VERTS_X-1;i++) { for (int j=0;j<NUM_VERTS_Y-1;j++) { gIndices[index++] = j * NUM_VERTS_X + i; gIndices[index++] = j * NUM_VERTS_X + i + 1; gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1; gIndices[index++] = j * NUM_VERTS_X + i; gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1; gIndices[index++] = (j + 1) * NUM_VERTS_X + i; } } if (BulletGlobals.g_streamWriter != null) { index = 0; BulletGlobals.g_streamWriter.WriteLine("setIndexPositions"); for (int i = 0; i < gIndices.Count; i++) { BulletGlobals.g_streamWriter.WriteLine(String.Format("{0} {1}", i, gIndices[i])); } } TriangleIndexVertexArray indexVertexArrays = new TriangleIndexVertexArray(totalTriangles, gIndices,indexStride,totalVerts,gVertices,vertStride); bool useQuantizedAabbCompression = true; OptimizedBvh bvh = new OptimizedBvh(); IndexedVector3 aabbMin = new IndexedVector3(-1000,-1000,-1000); IndexedVector3 aabbMax = new IndexedVector3(1000,1000,1000); m_trimeshShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression, ref aabbMin, ref aabbMax, true); //CollisionShape trimeshShape = new TriangleMeshShape(indexVertexArrays); IndexedVector3 scaling = IndexedVector3.One; //m_trimeshShape.SetOptimizedBvh(bvh, ref scaling); //BulletWorldImporter import = new BulletWorldImporter(0);//don't store info into the world //if (import.loadFile("myShape.bullet")) //{ // int numBvh = import.getNumBvhs(); // if (numBvh != 0) // { // OptimizedBvh bvh = import.getBvhByIndex(0); // IndexedVector3 aabbMin = new IndexedVector3(-1000,-1000,-1000); // IndexedVector3 aabbMax = new IndexedVector3(1000,1000,1000); // trimeshShape = new indexVertexArrays,useQuantizedAabbCompression,ref aabbMin,ref aabbMax,false); // IndexedVector3 scaling = IndexedVector3.One; // trimeshShape.setOptimizedBvh(bvh, ref scaling); // //trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,aabbMin,aabbMax); // //trimeshShape.setOptimizedBvh(bvh); // } // int numShape = import.getNumCollisionShapes(); // if (numShape != 0) // { // trimeshShape = (BvhTriangleMeshShape)import.getCollisionShapeByIndex(0); // //if you know the name, you can also try to get the shape by name: // String meshName = import.getNameForPointer(trimeshShape); // if (meshName != null) // { // trimeshShape = (BvhTriangleMeshShape)import.getCollisionShapeByName(meshName); // } // } //} //CollisionShape groundShape = trimeshShape;//m_trimeshShape; CollisionShape groundShape = m_trimeshShape;//m_trimeshShape; //groundShape = new TriangleShape(new IndexedVector3(0,` 0, 100), new IndexedVector3(100, 0, 0),new IndexedVector3(-100, 0, -100)); //groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f); //groundShape = new BoxShape(new IndexedVector3(100f, 0.1f, 100f)); IndexedVector3 up = new IndexedVector3(0.4f,1,0); up.Normalize(); //groundShape = new StaticPlaneShape(up, 0f); //groundShape = new TriangleMeshShape(indexVertexArrays); m_collisionConfiguration = new DefaultCollisionConfiguration(); m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); IndexedVector3 worldMin = new IndexedVector3(-1000,-1000,-1000); IndexedVector3 worldMax = new IndexedVector3(1000,1000,1000); //m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false); m_broadphase = new DbvtBroadphase(); m_constraintSolver = new SequentialImpulseConstraintSolver(); m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration); float mass = 0f; IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(2,-2,0)); //CompoundShape colShape = new CompoundShape(); //IndexedVector3 halfExtents = new IndexedVector3(4, 1, 1); //CollisionShape cylinderShape = new CylinderShapeX(ref halfExtents); //CollisionShape boxShape = new BoxShape(new IndexedVector3(4, 1, 1)); //IndexedMatrix localTransform = IndexedMatrix.Identity; //colShape.addChildShape(ref localTransform, boxShape); //Quaternion orn = Quaternion.CreateFromYawPitchRoll(MathUtil.SIMD_HALF_PI, 0f, 0f); //localTransform = IndexedMatrix.CreateFromQuaternion(orn); //colShape.addChildShape(ref localTransform, cylinderShape); ////BoxShape colShape = new BoxShape(new IndexedVector3(1, 1, 1)); //int numCollideObjects = 1; //m_collisionShapes.Add(colShape); //{ // for (int i = 0; i < numCollideObjects; i++) // { // startTransform._origin = new IndexedVector3(4,10+i*2,1); // localCreateRigidBody(1, ref startTransform,colShape); // } //} CollisionShape boxShape = new BoxShape(new IndexedVector3(1, 1, 1)); //CollisionShape boxShape = new SphereShape(1); //CollisionShape boxShape = new SphereShape(1); //CollisionShape boxShape = new CapsuleShapeZ(0.5f, 1); m_collisionShapes.Add(boxShape); for (int i = 0; i < 1; i++) { startTransform._origin = new IndexedVector3(2f * i, 5, 1); LocalCreateRigidBody(1, ref startTransform, boxShape); } startTransform = IndexedMatrix.Identity; staticBody = LocalCreateRigidBody(mass, ref startTransform,groundShape); staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT);//STATIC_OBJECT); //enable custom material callback staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK); //clientResetScene(); }
public bool Collide(ref IndexedVector3 sphereCenter, out IndexedVector3 point, out IndexedVector3 resultNormal, ref float depth, ref float timeOfImpact, float contactBreakingThreshold) { IndexedVector3[] vertices = m_triangle.GetVertexPtr(0); float radius = m_sphere.GetRadius(); float radiusWithThreshold = radius + contactBreakingThreshold; IndexedVector3 v1; IndexedVector3.Subtract(out v1,ref vertices[1],ref vertices[0]); IndexedVector3 v2; IndexedVector3.Subtract(out v2,ref vertices[2],ref vertices[0]); IndexedVector3 normal = new IndexedVector3(v1.Y * v2.Z - v1.Z * v2.Y,v1.Z * v2.X - v1.X * v2.Z,v1.X * v2.Y - v1.Y * v2.X); //IndexedVector3 normal = IndexedVector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]); normal.Normalize(); IndexedVector3 p1ToCentre; IndexedVector3.Subtract(out p1ToCentre,ref sphereCenter,ref vertices[0]); float distanceFromPlane = IndexedVector3.Dot(ref p1ToCentre, ref normal); if (distanceFromPlane < 0f) { //triangle facing the other way distanceFromPlane *= -1f; normal *= -1f; } bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold; // Check for contact / intersection bool hasContact = false; IndexedVector3 contactPoint = IndexedVector3.Zero; if (isInsideContactPlane) { if (FaceContains(ref sphereCenter, vertices, ref normal)) { // Inside the contact wedge - touches a point on the shell plane hasContact = true; contactPoint = sphereCenter - normal * distanceFromPlane; } else { // Could be inside one of the contact capsules float contactCapsuleRadiusSqr = (radiusWithThreshold) * (radiusWithThreshold); IndexedVector3 nearestOnEdge; for (int i = 0; i < m_triangle.GetNumEdges(); i++) { IndexedVector3 pa; IndexedVector3 pb; m_triangle.GetEdge(i, out pa, out pb); float distanceSqr = SegmentSqrDistance(ref pa, ref pb, ref sphereCenter, out nearestOnEdge); if (distanceSqr < contactCapsuleRadiusSqr) { // Yep, we're inside a capsule hasContact = true; contactPoint = nearestOnEdge; } } } } if (hasContact) { IndexedVector3 contactToCentre = sphereCenter - contactPoint; float distanceSqr = contactToCentre.LengthSquared(); if (distanceSqr < (radiusWithThreshold) * (radiusWithThreshold)) { if (distanceSqr > MathUtil.SIMD_EPSILON) { float distance = (float)Math.Sqrt(distanceSqr); resultNormal = contactToCentre; resultNormal.Normalize(); point = contactPoint; depth = -(radius - distance); } else { float distance = 0.0f; resultNormal = normal; point = contactPoint; depth = -radius; } return true; } } resultNormal = new IndexedVector3(0, 1, 0); point = IndexedVector3.Zero; return false; }