public override void Iterate() { DeltaVelocity = TargetVelocity - Body1.LinearVelocity; DeltaVelocity.Y = 0.0f; var fraction = 0.02f; if (WalkingOn == null) fraction = 0.0001f; DeltaVelocity *= fraction; if (DeltaVelocity.LengthSquared() != 0.0f) { Body1.IsActive = true; Body1.ApplyImpulse(DeltaVelocity * Body1.Mass); } if (ShouldJump) { Body1.IsActive = true; Body1.ApplyImpulse(JumpVelocity * JVector.Up * Body1.Mass); if (!WalkingOn.IsStatic) { WalkingOn.IsActive = true; WalkingOn.ApplyImpulse(-1.0f * JumpVelocity * JVector.Up * Body1.Mass); } } }
public override void Iterate() { deltaVelocity = TargetVelocity - Body1.LinearVelocity; deltaVelocity.Y = 0.0f; // determine how 'stiff' the character follows the target velocity deltaVelocity *= 0.02f; if (deltaVelocity.LengthSquared() != 0.0f) { // activate it, in case it fall asleep :) Body1.IsActive = true; Body1.ApplyImpulse(deltaVelocity * Body1.Mass); } if (shouldIJump) { Body1.IsActive = true; Body1.ApplyImpulse(jumpVelocity * JVector.Up * Body1.Mass); if (!BodyWalkingOn.IsStatic) { BodyWalkingOn.IsActive = true; // apply the negative impulse to the other body BodyWalkingOn.ApplyImpulse(-1.0f * jumpVelocity * JVector.Up * Body1.Mass); } } }
/// <summary> /// Initializes a new instance of the WorldLineConstraint. /// </summary> /// <param name="body">The body of the constraint.</param> /// <param name="localAnchor">The anchor point on the body in local (body) /// coordinates.</param> /// <param name="lineDirection">The axis defining the line in world space.</param>/param> public PointOnLine(RigidBody body, JVector localAnchor, JVector lineDirection) : base(body, null) { if (lineDirection.LengthSquared() == 0.0f) throw new ArgumentException("Line direction can't be zero", "lineDirection"); localAnchor1 = localAnchor; this.anchor = body.position + JVector.Transform(localAnchor, body.orientation); this.lineNormal = lineDirection; this.lineNormal.Normalize(); }
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1, ref JMatrix orientation2, ref JVector position1, ref JVector position2, out JVector p1, out JVector p2, out JVector normal) { VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); p1 = p2 = JVector.Zero; JVector r = position1 - position2; JVector w, v; JVector supVertexA; JVector rn,vn; rn = JVector.Negate(r); SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA); JVector supVertexB; SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB); v = supVertexA - supVertexB; normal = JVector.Zero; int maxIter = 15; float distSq = v.LengthSquared(); float epsilon = 0.00001f; while ((distSq > epsilon) && (maxIter-- != 0)) { vn = JVector.Negate(v); SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA); SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB); w = supVertexA - supVertexB; if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB); if (simplexSolver.Closest(out v)) { distSq = v.LengthSquared(); normal = v; } else distSq = 0.0f; } simplexSolver.ComputePoints(out p1, out p2); if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon) normal.Normalize(); simplexSolverPool.GiveBack(simplexSolver); return true; }
// see: btSubSimplexConvexCast.cpp /// <summary> /// Checks if a ray definied through it's origin and direction collides /// with a shape. /// </summary> /// <param name="support">The supportmap implementation representing the shape.</param> /// <param name="orientation">The orientation of the shape.</param> /// <param name="invOrientation">The inverse orientation of the shape.</param> /// <param name="position">The position of the shape.</param> /// <param name="origin">The origin of the ray.</param> /// <param name="direction">The direction of the ray.</param> /// <param name="fraction">The fraction which gives information where at the /// ray the collision occured. The hitPoint is calculated by: origin+friction*direction.</param> /// <param name="normal">The normal from the ray collision.</param> /// <returns>Returns true if the ray hit the shape, false otherwise.</returns> public static bool Raycast(ISupportMappable support, ref JMatrix orientation, ref JMatrix invOrientation, ref JVector position,ref JVector origin,ref JVector direction, out float fraction, out JVector normal) { VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); normal = JVector.Zero; fraction = float.MaxValue; float lambda = 0.0f; JVector r = direction; JVector x = origin; JVector w, p, v; JVector arbitraryPoint; SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint); JVector.Subtract(ref x, ref arbitraryPoint, out v); int maxIter = MaxIterations; float distSq = v.LengthSquared(); float epsilon = 0.000001f; float VdotR; while ((distSq > epsilon) && (maxIter-- != 0)) { SupportMapTransformed(support, ref orientation, ref position, ref v, out p); JVector.Subtract(ref x, ref p, out w); float VdotW = JVector.Dot(ref v, ref w); if (VdotW > 0.0f) { VdotR = JVector.Dot(ref v, ref r); if (VdotR >= -JMath.Epsilon) { simplexSolverPool.GiveBack(simplexSolver); return false; } else { lambda = lambda - VdotW / VdotR; JVector.Multiply(ref r, lambda, out x); JVector.Add(ref origin, ref x, out x); JVector.Subtract(ref x, ref p, out w); normal = v; } } if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, x, p); if (simplexSolver.Closest(out v)) { distSq = v.LengthSquared(); } else distSq = 0.0f; } #region Retrieving hitPoint // Giving back the fraction like this *should* work // but is inaccurate against large objects: // fraction = lambda; JVector p1, p2; simplexSolver.ComputePoints(out p1, out p2); p2 = p2 - origin; fraction = p2.Length() / direction.Length(); #endregion if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon) normal.Normalize(); simplexSolverPool.GiveBack(simplexSolver); return true; }
public override void Iterate() { _deltaVelocity = TargetVelocity - Body1.LinearVelocity; _deltaVelocity.Y = 0.0f; _deltaVelocity *= Stiffness; if (Math.Abs(_deltaVelocity.LengthSquared()) > 0.00001f) { Body1.IsActive = true; Body1.ApplyImpulse(_deltaVelocity * Body1.Mass); } if (_shouldIJump) { Body1.IsActive = true; Body1.ApplyImpulse(JumpVelocity * JVector.Up * Body1.Mass); if (!BodyWalkingOn.IsStatic) { BodyWalkingOn.IsActive = true; BodyWalkingOn.ApplyImpulse(-1.0f * JumpVelocity * JVector.Up * Body1.Mass); } } }
public override void Iterate() { deltaVelocity = TargetVelocity - Body1.LinearVelocity; deltaVelocity.Y = 0.0f; // determine how 'stiff' the character follows the target velocity deltaVelocity *= 0.005f; if (deltaVelocity.LengthSquared() != 0.0f) { // activate it, in case it fall asleep :) Body1.IsActive = true; Body1.ApplyImpulse(deltaVelocity * Body1.Mass); } if (shouldIJump) { Body1.IsActive = true; Body1.ApplyImpulse(JumpVelocity * JVector.Up * Body1.Mass); System.Diagnostics.Debug.WriteLine("JUMP! " + DateTime.Now.Second.ToString()); if (!BodyWalkingOn.IsStatic) { BodyWalkingOn.IsActive = true; // apply the negative impulse to the other body BodyWalkingOn.ApplyImpulse(-1.0f * JumpVelocity * JVector.Up * Body1.Mass); } } }