private void TryMoveUp(float ifps) { usedAutoStepping = false; lastStepHeight = 0; if (!IsFrozen && MaxStepHeight > 0) // when player is moving { // move (apply "position") dummy.Transform = GetBodyTransform(); // find collisions with the capsule shape.GetCollision(contacts, 0.0f); if (contacts.Count == 0) { return; } int contactsCount = MathLib.Min(contacts.Count, contactsBufferSize); // find max step height for (int i = 0; i < contactsCount; i++) { ShapeContact c = contacts[i]; vec3 normalXY = c.Normal - vec3.UP * MathLib.Dot(vec3.UP, c.Normal); vec3 velocityXY = velocity - vec3.UP * MathLib.Dot(vec3.UP, velocity); // skip contacts opposite to movement if (normalXY.Length2 > MathLib.EPSILON && velocityXY.Length2 > MathLib.EPSILON && MathLib.Dot(normalXY.Normalized, velocityXY.Normalized) > 0.5f) { continue; } float step = MathLib.Dot(new vec3(c.Point - position), vec3.UP); if (lastStepHeight < step) { lastStepHeight = step; } } // apply auto stepping if (MinStepHeight < lastStepHeight && lastStepHeight < MaxStepHeight) { position += new vec3(zAxis * lastStepHeight); autoSteppingOffset += new vec3(zAxis * lastStepHeight); usedAutoStepping = true; } if (renderUpPassContacts) { foreach (var c in contacts) { Visualizer.RenderVector(c.Point, c.Point + c.Normal, new vec4(1, 0, 0, 1)); } } } }
private void UpdateTransform() { // ortho basis vec3 tangent, binormal; Geometry.OrthoBasis(vec3.UP, out tangent, out binormal); // decompose transformation position = node.WorldPosition - zAxis * (collisionHeight + 2.0f * CollisionRadius); direction = MathLib.Normalize(new vec3(-node.WorldTransform.AxisZ)); // decompose direction // in this case don't use properties phiAngle = MathLib.Atan2(MathLib.Dot(direction, tangent), MathLib.Dot(direction, binormal)) * MathLib.RAD2DEG; thetaAngle = MathLib.Acos(MathLib.Clamp(MathLib.Dot(direction, vec3.UP), -1.0f, 1.0f)) * MathLib.RAD2DEG - 90.0f; // object transformation playerBody.WorldTransform = GetBodyTransform(); // set camera camera.Modelview = GetModelview(); }
private void MoveSide(float ifps) { vec3 tangent = vec3.ZERO; vec3 binormal = vec3.ZERO; for (int i = 0; i < collisionIterations; i++) { // move (apply "position") dummy.Transform = GetBodyTransform(); // find collisions with the capsule shape.GetCollision(contacts, 0.0f); // in case of a large number of contacts, we use one iteration to avoid performance degradation if (contacts.Count > heavyContactsCount) { i = collisionIterations - 1; } if (contacts.Count == 0) { break; } int contactsCount = MathLib.Min(contacts.Count, contactsBufferSize); float inumContacts = 1.0f / MathLib.Itof(contactsCount); float maxSlopeAngle = -MathLib.INFINITY; // solving collisions for (int j = 0; j < contactsCount; j++) { ShapeContact c = contacts[j]; FixContact(ref c); float normalSpeed = 0.0f; if (IsFrozen) { position += zAxis * c.Depth * inumContacts * MathLib.Dot(zAxis, c.Normal); autoSteppingOffset += zAxis * c.Depth * inumContacts * MathLib.Dot(zAxis, c.Normal); normalSpeed = MathLib.Dot(zAxis, velocity); velocity -= zAxis * normalSpeed; } else { position += c.Normal * c.Depth * inumContacts; autoSteppingOffset += c.Normal * c.Depth * inumContacts; IsFrozen = false; normalSpeed = MathLib.Dot(c.Normal, velocity); velocity -= c.Normal * normalSpeed; } // friction if (targetSpeed < MathLib.EPSILON) { Geometry.OrthoBasis(c.Normal, out tangent, out binormal); float tangentSpeed = MathLib.Dot(tangent, velocity); float binormalSpeed = MathLib.Dot(binormal, velocity); if (MathLib.Abs(tangentSpeed) > MathLib.EPSILON || MathLib.Abs(binormalSpeed) > MathLib.EPSILON) { float frictionSpeed = MathLib.Max(-normalSpeed, 0.0f) * friction * MathLib.Rsqrt(tangentSpeed * tangentSpeed + binormalSpeed * binormalSpeed); frictionSpeed = MathLib.Clamp(frictionSpeed, -1.0f, 1.0f); velocity -= tangent * tangentSpeed * frictionSpeed; velocity -= binormal * binormalSpeed * frictionSpeed; } } if (MathLib.Dot(c.Normal, vec3.UP) > 0.5f && MathLib.Dot(new vec3(c.Point - shape.BottomCap), vec3.UP) < 0.0f) { IsGround = true; float angle = MathLib.GetAngle(vec3.UP, c.Normal); if (angle > maxSlopeAngle && 0 < angle && angle < 90.0f) { slopeNormal = c.Normal; slopePoint = c.Point; slopeAngle = angle; maxSlopeAngle = angle; } } if (MathLib.Dot(c.Normal, vec3.UP) < -0.5f && MathLib.Dot(new vec3(c.Point - shape.TopCap), vec3.UP) > 0.0f) { IsCeiling = true; } } if (renderSidePassContacts) { foreach (var c in contacts) { Visualizer.RenderVector(c.Point, c.Point + c.Normal, new vec4(0, 1, 1, 1)); } } } }
private void UpdatePosition(float ifps) { position += new vec3(velocity * ifps); }
private void Init() { if (showAdvancedSettings) { showAdvancedSettings = false; } if (useExternalBody) { useExternalBody = false; } PlayerDummy player = node as PlayerDummy; if (!player) { return; } // decompose transformation position = node.WorldPosition; direction = MathLib.Normalize(new vec3(-node.WorldTransform.AxisZ)); camera = player.Camera; if (playerBody) { dummy = playerBody.Body as BodyDummy; if (dummy) { for (int i = 0; i < dummy.NumShapes; i++) { if (!shape) { shape = dummy.GetShape(i) as ShapeCapsule; } } if (shape) { shape.Restitution = 0.0f; shape.Continuous = false; PhysicalIntersectionMask = shape.PhysicsIntersectionMask; CollisionMask = shape.CollisionMask; Mass = shape.Mass; CollisionRadius = shape.Radius; CollisionHeight = shape.Height; } } } if (!dummy || !shape) { if (playerBody) { playerBody.Enabled = false; } playerBody = new ObjectDummy(); dummy = new BodyDummy(); shape = new ShapeCapsule(1.0f, 1.0f); shape.Restitution = 0.0f; shape.Continuous = false; dummy.Enabled = true; playerBody.Body = dummy; shape.Body = dummy; PhysicalIntersectionMask = physicalIntersectionMask; CollisionMask = collisionMask; Mass = mass; CollisionRadius = collisionRadius; CollisionHeight = collisionHeight; } contacts = new List <ShapeContact>(); UpdateTransform(); maxSlopeAngle = MathLib.Max(maxSlopeAngle, maxStaticSlopeAngle); maxSlidingSpeed = MathLib.Max(maxSlidingSpeed, minSlidingSpeed); if (showDebug) { Visualizer.Enabled = true; Render.ShowTriangles = 1; } isInitialized = true; }
/// <summary> /// This function fixed contacts only with external objects for capsule /// </summary> private void FixContact(ref ShapeContact contact) { Vec3 firstPoint = Vec3.ZERO; // set first point for intersection // it's bottom or top sphere of capsule // or cylinder part of capsule if (contact.Point.z < shape.BottomCap.z) { firstPoint = shape.BottomCap; } else if (shape.BottomCap.z <= contact.Point.z && contact.Point.z <= shape.TopCap.z) { firstPoint = new Vec3(shape.TopCap.x, shape.TopCap.y, contact.Point.z); } else { firstPoint = shape.TopCap; } // try get contact object WorldIntersectionNormal normalIntersection = new WorldIntersectionNormal(); var hitObj = World.GetIntersection(firstPoint, contact.Point, contactsIntersectionMask, normalIntersection); if (hitObj) { // get real distance to contact float distance = (float)(normalIntersection.Point - firstPoint).Length; if (distance < shape.Radius) { // set correct parameters for contact contact.Point = normalIntersection.Point; contact.Depth = shape.Radius - distance; contact.Normal = new vec3((firstPoint - contact.Point).Normalize()); } else { // contact outside capsule contact.Point = Vec3.ZERO; contact.Depth = 0.0f; contact.Normal = vec3.ZERO; } } // check contact with horizontal plane, because intersection can't detect it else if (MathLib.Dot(vec3.UP, contact.Normal) < MathLib.EPSILON) { if (contact.Object) { vec3 scale = contact.Object.WorldScale; // check object scale if (MathLib.Compare(scale.x, 1.0f) != 1 || MathLib.Compare(scale.y, 1.0f) != 1 || MathLib.Compare(scale.z, 1.0f) != 1) { // this value avoids jittering contact.Depth = MathLib.Max(Physics.PenetrationTolerance, 0.01f); } } } else { // capsule does not contact with this object contact.Point = Vec3.ZERO; contact.Depth = 0.0f; contact.Normal = vec3.ZERO; } }
private void TryMoveDown(float ifps) { vec3 moveImpulseXY = moveImpulse - vec3.UP * MathLib.Dot(vec3.UP, moveImpulse); Vec3 pos1 = position + (moveImpulseXY.Normalized + vec3.UP) * shape.Radius; Vec3 pos2 = pos1 - vec3.UP * shape.Radius; WorldIntersectionNormal intersection = new WorldIntersectionNormal(); Unigine.Object hitObj = World.GetIntersection(pos1, pos2, contactsIntersectionMask, intersection); if (hitObj) { float angle = MathLib.GetAngle(vec3.UP, intersection.Normal); if (autoSteppingCancelAngle < angle && angle < 90.0f) { position -= autoSteppingOffset; usedAutoStepping = false; return; } } vec3 velocityXY = velocity - vec3.UP * MathLib.Dot(vec3.UP, velocity); pos1 = position + (velocityXY.Normalized + vec3.UP) * shape.Radius; pos2 = pos1 - vec3.UP * shape.Radius; hitObj = World.GetIntersection(pos1, pos2, contactsIntersectionMask, intersection); if (hitObj) { float angle = MathLib.GetAngle(vec3.UP, intersection.Normal); if (autoSteppingCancelAngle < angle && angle < 90.0f) { position -= autoSteppingOffset; usedAutoStepping = false; return; } } // this correction allows to avoid jittering on large stairs if (lastStepHeight > shape.Radius) { if (GravityMultiplier > 1.0f) { lastStepHeight = shape.Radius / GravityMultiplier - Physics.PenetrationTolerance; } else { lastStepHeight = shape.Radius - Physics.PenetrationTolerance; } } // try to drop down the player position -= new vec3(zAxis * lastStepHeight); autoSteppingOffset -= new vec3(zAxis * lastStepHeight); // move (apply "position") dummy.Transform = GetBodyTransform(); // find collisions with the capsule shape.GetCollision(contacts, 0.0f); if (contacts.Count == 0) { return; } int contactsCount = MathLib.Min(contacts.Count, contactsBufferSize); float inumContacts = 1.0f / MathLib.Itof(contactsCount); // push up (if collisions exists) for (int i = 0; i < contactsCount; i++) { ShapeContact c = contacts[i]; position += new vec3(zAxis * (MathLib.Max(c.Depth, 0.0f) * inumContacts * MathLib.Dot(zAxis, c.Normal))); if (MathLib.Dot(c.Normal, vec3.UP) > 0.5f && MathLib.Dot(new vec3(c.Point - shape.BottomCap), vec3.UP) < 0.0f) { IsGround = true; } } if (renderDownPassContacts) { foreach (var c in contacts) { Visualizer.RenderVector(c.Point, c.Point + c.Normal, new vec4(0, 1, 0, 1)); } } }
public void Hack_UpdatePosition(vec3 newPosition) { position = newPosition; }