static Vector3 ParallelComponent(ref Vector3 direction, ref Vector3 normal) { float magnitude; Vector3.Dot(ref direction, ref normal, out magnitude); return(normal * magnitude); }
static Vector3 ComputeReflectionDirection(ref Vector3 direction, ref Vector3 normal) { float dot; Vector3.Dot(ref direction, ref normal, out dot); return(direction - (2.0f * dot) * normal); }
public void StepUp(float dt) { Matrix transform = RigidBody.MotionState.WorldTransform; if (!_stepping) { _stepping = true; _steppingTo.Y += CapsuleHalfHeight + CapsuleRadius; } /* Bullet reacts with velocity also with kinematic bodies, althogh this * should not change anything, until we do not go back to dynamic body. */ Vector3 origin = transform.Origin; float hor = Vector3.Dot(origin - _steppingTo, _steppingInvNormal); Vector3 stepDir = _steppingTo - origin; stepDir.Y = 0; stepDir.Normalize(); float speed = Vector3.Dot(stepDir, Vector3.TransformNormal(_targetSpeed, transform.Basis)) * SteppingSpeed; origin.Y += speed * dt * 2; float dv = _steppingTo.Y - origin.Y; if (dv <= -1 * MaxStepHeight || _targetSpeed.LengthSquared < 0.0001f || speed <= 0) { CancelStep(); } else if (dv < CapsuleRadius) { float dh = (float)System.Math.Sqrt(dv * (2 * CapsuleRadius - dv)); if (dh < System.Math.Abs(hor)) { float advance = System.Math.Min(dh * System.Math.Sign(hor) - hor, speed * dt); var addVec = _steppingInvNormal * advance; transform.Origin = transform.Origin + addVec; } } RigidBody.MotionState.WorldTransform = transform; }
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove) { Matrix start = Matrix.Identity, end = Matrix.Identity; targetPosition = currentPosition + walkMove; float fraction = 1.0f; float distance2 = (currentPosition - targetPosition).LengthSquared; // printf("distance2=%f\n",distance2); if (touchingContact) { float dot; Vector3.Dot(ref normalizedDirection, ref m_touchingNormal, out dot); if (dot > 0.0f) { //interferes with step movement //UpdateTargetPositionBasedOnCollision(ref m_touchingNormal, 0.0f, 1.0f); } } int maxIter = 10; while (fraction > 0.01f && maxIter-- > 0) { start.Origin = (currentPosition); end.Origin = (targetPosition); Vector3 sweepDirNegative = currentPosition - targetPosition; KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(ghostObject, sweepDirNegative, 0f); callback.CollisionFilterGroup = ghostObject.BroadphaseHandle.CollisionFilterGroup; callback.CollisionFilterMask = ghostObject.BroadphaseHandle.CollisionFilterMask; float margin = convexShape.Margin; convexShape.Margin = margin + .02f; ghostObject.ConvexSweepTestRef(convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); convexShape.Margin = margin; fraction -= callback.ClosestHitFraction; if (callback.HasHit) { // We moved only a fraction float hitDistance = (callback.HitPointWorld - currentPosition).Length; Vector3 hitNormalWorld = callback.HitNormalWorld; UpdateTargetPositionBasedOnCollision(ref hitNormalWorld, 0f, 1f); Vector3 currentDir = targetPosition - currentPosition; distance2 = currentDir.LengthSquared; if (distance2 > MathUtil.SIMD_EPSILON) { currentDir.Normalize(); /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ float dot; Vector3.Dot(ref currentDir, ref normalizedDirection, out dot); if (dot <= 0.0f) { break; } } else { break; } } else { // we moved whole way currentPosition = targetPosition; } } }