/// <summary> /// Called from Simulate /// This is the avatar's movement control + PID Controller /// </summary> /// <param name="timeStep"></param> public void Move(float timeStep) { // no lock; for now it's only called from within Simulate() // If the PID Controller isn't active then we set our force // calculating base velocity to the current position if (Body == null) { return; } tempTrans1.Dispose(); tempTrans1 = Body.getInterpolationWorldTransform(); tempVector1.Dispose(); tempVector1 = tempTrans1.getOrigin(); tempVector2.Dispose(); tempVector2 = Body.getInterpolationLinearVelocity(); if (m_pidControllerActive == false) { m_zeroPosition.X = tempVector1.getX(); m_zeroPosition.Y = tempVector1.getY(); m_zeroPosition.Z = tempVector1.getZ(); } //PidStatus = true; Vector3 vec = Vector3.Zero; Vector3 vel = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ()); vel *= 0.25f; float movementdivisor = 1f; if (!m_alwaysRun) { movementdivisor = walkDivisor; } else { movementdivisor = runDivisor; } // if velocity is zero, use position control; otherwise, velocity control if (m_target_velocity.X == 0.0f && m_target_velocity.Y == 0.0f && m_target_velocity.Z == 0.0f && m_iscolliding) { // keep track of where we stopped. No more slippin' & slidin' if (!m_zeroFlag) { m_zeroFlag = true; m_zeroPosition.X = tempVector1.getX(); m_zeroPosition.Y = tempVector1.getY(); m_zeroPosition.Z = tempVector1.getZ(); } if (m_pidControllerActive) { // We only want to deactivate the PID Controller if we think we want to have our surrogate // react to the physics scene by moving it's position. // Avatar to Avatar collisions // Prim to avatar collisions Vector3 pos = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ()); vec.X = (m_target_velocity.X - vel.X) * (PID_D) + (m_zeroPosition.X - pos.X) * (PID_P * 2); vec.Y = (m_target_velocity.Y - vel.Y) * (PID_D) + (m_zeroPosition.Y - pos.Y) * (PID_P * 2); if (m_flying) { vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D) + (m_zeroPosition.Z - pos.Z) * PID_P; } } //PidStatus = true; } else { m_pidControllerActive = true; m_zeroFlag = false; if (m_iscolliding && !m_flying) { // We're standing on something vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D); vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D); } else if (m_iscolliding && m_flying) { // We're flying and colliding with something vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16); vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16); } else if (!m_iscolliding && m_flying) { // we're in mid air suspended vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6); vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6); // We don't want linear velocity to cause our avatar to bounce, so we check target Z and actual velocity X, Y // rebound preventing //if (m_target_velocity.Z < 0.025f && m_velocity.X < 0.25f && m_velocity.Y < 0.25f) // m_zeroFlag = true; } if (m_iscolliding && !m_flying && m_target_velocity.Z > 0.0f) { // We're colliding with something and we're not flying but we're moving // This means we're walking or running. Vector3 pos = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ()); vec.Z = (m_target_velocity.Z - vel.Z) * PID_D + (m_zeroPosition.Z - pos.Z) * PID_P; if (m_target_velocity.X > 0) { vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D; } if (m_target_velocity.Y > 0) { vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D; } } else if (!m_iscolliding && !m_flying) { // we're not colliding and we're not flying so that means we're falling! // m_iscolliding includes collisions with the ground. // d.Vector3 pos = d.BodyGetPosition(Body); if (m_target_velocity.X > 0) { vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D; } if (m_target_velocity.Y > 0) { vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D; } } if (m_flying) { vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D) * 10; } } if (m_flying) { // Slight PID correction vec.Z += (((-1 * m_parent_scene.gravityz) * m_mass) * 1.5f); //auto fly height. Kitto Flora //d.Vector3 pos = d.BodyGetPosition(Body); float target_altitude = m_parent_scene.GetTerrainHeightAtXY(m_position.X, m_position.Y) + m_parent_scene.minimumGroundFlightOffset; if (m_position.Z < target_altitude) { vec.Z += (target_altitude - m_position.Z) * PID_P * 5.0f; } } if (Body != null && (((m_target_velocity.X > 0.2f || m_target_velocity.X < -0.2f) || (m_target_velocity.Y > 0.2f || m_target_velocity.Y < -0.2f)))) { Body.setFriction(0.001f); //m_log.DebugFormat("[PHYSICS]: Avatar force applied: {0}, Target:{1}", vec.ToString(), m_target_velocity.ToString()); } if (Body != null) { int activationstate = Body.getActivationState(); if (activationstate == 0) { Body.forceActivationState(1); } } doImpulse(vec, true); }
static void Main(string[] args) { btVector3 testvec = new btVector3(-2, 1, 0); Console.WriteLine(String.Format("Original: {0}", testvec.testStr())); btVector3 testvec2 = testvec.absolute(); Console.WriteLine(String.Format("absolute: {0}", testvec2.testStr())); Console.WriteLine(String.Format("angle:{0}", testvec.angle(testvec2))); Console.WriteLine(String.Format("closestAxis(orig):{0}", testvec.closestAxis())); btVector3 testvec3 = testvec.cross(testvec2); Console.WriteLine(String.Format("cross: {0}", testvec3.testStr())); Console.WriteLine(String.Format("distance: {0}", testvec.distance(testvec2))); Console.WriteLine(String.Format("distance2: {0}", testvec.distance2(testvec2))); Console.WriteLine(String.Format("dot: {0}", testvec.dot(testvec2))); Console.WriteLine(String.Format("furthestAxis(orig): {0}", testvec.furthestAxis())); btVector3 testvec4 = testvec.normalized(); Console.WriteLine(String.Format("normalized: {0}", testvec4.testStr())); testvec4.setInterpolate3(testvec, testvec2, 0.5f); Console.WriteLine(String.Format("interpolate3: {0}", testvec4.testStr())); testvec4.setValue(7f, -0.09f, 2.5f); Console.WriteLine(String.Format("setvec: {0}", testvec4.testStr())); testvec4.setX(5.0f); testvec4.setY(-0.25f); testvec4.setZ(90f); testvec.setValue(0, 0, -1024); testvec2.setValue(256, 256, 1024); Console.WriteLine(String.Format("setvecIndividual: {0}", testvec4.testStr())); btAxisSweep3 testbtAxisSweep3 = new btAxisSweep3(testvec, testvec2, 50); btDefaultCollisionConfiguration colconfig = new btDefaultCollisionConfiguration(); btCollisionDispatcher coldisp = new btCollisionDispatcher(colconfig); btSequentialImpulseConstraintSolver seqimpconssol = new btSequentialImpulseConstraintSolver(); btDiscreteDynamicsWorld dynamicsWorld = new btDiscreteDynamicsWorld(coldisp, testbtAxisSweep3, seqimpconssol, colconfig); dynamicsWorld.setGravity(new btVector3(0, 0, -9.87f)); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60)))); btQuaternion testquat = new btQuaternion(50, 0, 0, 1); btQuaternion testquatnorm = testquat.normalized(); Console.WriteLine(String.Format("testquat: {0}", testquat.testStr())); Console.WriteLine(String.Format("testquatnormalize: {0}", testquatnorm.testStr())); Console.WriteLine(String.Format("testquatLength: {0}", testquat.length())); Console.WriteLine(String.Format("testquatnormalizeLength: {0}", testquatnorm.length())); float[] heightdata = new float[256 * 256]; for (int j = 0; j < 256 * 256; j++) { if (j % 2 == 0) { heightdata[j] = 21f; } else { heightdata[j] = 28f; } } btHeightfieldTerrainShape obj = new btHeightfieldTerrainShape(256, 256, heightdata, 1.0f, 0, 256, (int)btHeightfieldTerrainShape.UPAxis.Z, (int)btHeightfieldTerrainShape.PHY_ScalarType. PHY_FLOAT, false); btCapsuleShape cap = new btCapsuleShape(0.23f, 3); btTriangleMesh testMesh = new btTriangleMesh(true, false); testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false); testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(-1, -1, -1), false); testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, 0, 1), new btVector3(-1, -1, -1), false); testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(-1, 0, -1), false); testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(-1, 0, -1), new btVector3(-1, -1, -1), false); testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false); testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(1, 0, -1), false); testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(1, 0, 1), false); btGImpactMeshShape meshtest = new btGImpactMeshShape(testMesh); meshtest.updateBound(); btRigidBody groundbody = new btRigidBody(0, new btDefaultMotionState( new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(128, 128, 256f / 2f))), obj, new btVector3(0, 0, 0)); btRigidBody capbody = new btRigidBody(200, new btDefaultMotionState( new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(128, 128, 25))), cap, new btVector3(0, 0, 0)); btRigidBody meshbody = new btRigidBody(200, new btDefaultMotionState( new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(128, 128, 29))), meshtest, new btVector3(0, 0, 0)); btRigidBodyConstructionInfo constructioninfotest = new btRigidBodyConstructionInfo(); constructioninfotest.m_collisionShape = new btBoxShape(new btVector3(0.5f, 0.5f, 0.5f)); constructioninfotest.m_localInertia = new btVector3(0, 0, 0); constructioninfotest.m_motionState = new btDefaultMotionState(new btTransform(new btQuaternion(0.3f, -0.4f, 0.8f, 0.1f), new btVector3(128.5f, 128, 25)), new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, 0.25f, 0))); constructioninfotest.m_startWorldTransform = new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, 0, 0)); constructioninfotest.m_mass = 2000000; constructioninfotest.m_linearDamping = 0; constructioninfotest.m_angularDamping = 0; constructioninfotest.m_friction = 0.1f; constructioninfotest.m_restitution = 0; constructioninfotest.m_linearSleepingThreshold = 0.8f; constructioninfotest.m_angularSleepingThreshold = 1; constructioninfotest.m_additionalDamping = false; constructioninfotest.m_additionalDampingFactor = 0.005f; constructioninfotest.m_additionalLinearDampingThresholdSqr = 0.01f; constructioninfotest.m_additionalAngularDampingThresholdSqr = 0.01f; constructioninfotest.m_additionalAngularDampingFactor = 0.01f; constructioninfotest.commit(); btGImpactCollisionAlgorithm.registerAlgorithm(coldisp); btRigidBody cubetest = new btRigidBody(constructioninfotest); dynamicsWorld.addRigidBody(groundbody); dynamicsWorld.addRigidBody(cubetest); dynamicsWorld.addRigidBody(capbody); dynamicsWorld.addRigidBody(meshbody); int frame = 0; for (int i = 0; i < 26; i++) { int frames = dynamicsWorld.stepSimulation(((i % 60) / 60f), 10, (1f / 60)); frame += frames; Console.WriteLine(String.Format("Cube: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, cubetest.getInterpolationWorldTransform().getOrigin().testStr(), cubetest.getWorldTransform().getRotation().testStr())); Console.WriteLine(String.Format("Cap: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, capbody.getInterpolationWorldTransform().getOrigin().testStr(), capbody.getWorldTransform().getRotation().testStr())); Console.WriteLine(String.Format("Mesh: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, meshbody.getInterpolationWorldTransform().getOrigin().testStr(), meshbody.getWorldTransform().getRotation().testStr())); } dynamicsWorld.removeRigidBody(meshbody); dynamicsWorld.removeRigidBody(capbody); dynamicsWorld.removeRigidBody(cubetest); dynamicsWorld.removeRigidBody(groundbody); cubetest.Dispose(); groundbody.Dispose(); capbody.Dispose(); cap.Dispose(); obj.Dispose(); testbtAxisSweep3.Dispose(); dynamicsWorld.Dispose(); coldisp.Dispose(); colconfig.Dispose(); seqimpconssol.Dispose(); testvec.Dispose(); testvec2.Dispose(); testvec3.Dispose(); testvec4.Dispose(); }