Exemple #1
0
        /// <summary>
        /// Adds a wheel to the BRaycastVehicle from the given information.
        /// </summary>
        /// <param name="connectionPoint"></param>
        /// <param name="axle"></param>
        /// <param name="suspensionRestLength"></param>
        /// <param name="radius"></param>
        /// <returns></returns>
        public int AddWheel(WheelType wheelType, BulletSharp.Math.Vector3 connectionPoint, BulletSharp.Math.Vector3 axle, float suspensionRestLength, float radius)
        {
            float slidingFriction = DefaultSlidingFriction;

            switch (wheelType)
            {
            case WheelType.MECANUM:
                slidingFriction = 0.1f;
                axle            = (Quaternion.AngleAxis((connectionPoint.X > 0 && connectionPoint.Z > 0) || (connectionPoint.X < 0 && connectionPoint.Z < 0) ? -45 : 45,
                                                        Vector3.up) * axle.ToUnity()).ToBullet();
                break;

            case WheelType.OMNI:
                slidingFriction = 0.1f;
                break;
            }

            RobotWheelInfo wheel = RaycastRobot.AddWheel(connectionPoint,
                                                         -BulletSharp.Math.Vector3.UnitY, axle, suspensionRestLength,
                                                         radius, defaultVehicleTuning, false);

            wheel.RollInfluence   = RollInfluence;
            wheel.SlidingFriction = slidingFriction;

            return(RaycastRobot.NumWheels - 1);
        }
Exemple #2
0
        /// <summary>
        /// Adds a wheel to the RaycastRobot.
        /// </summary>
        /// <param name="connectionPointCS"></param>
        /// <param name="wheelDirectionCS0"></param>
        /// <param name="wheelAxleCS"></param>
        /// <param name="suspensionRestLength"></param>
        /// <param name="wheelRadius"></param>
        /// <param name="tuning"></param>
        /// <param name="isFrontWheel"></param>
        /// <returns></returns>
        public RobotWheelInfo AddWheel(Vector3 connectionPointCS, Vector3 wheelDirectionCS0, Vector3 wheelAxleCS,
                                       float suspensionRestLength, float wheelRadius, VehicleTuning tuning, bool isFrontWheel)
        {
            WheelInfoConstructionInfo ci = new WheelInfoConstructionInfo
            {
                ChassisConnectionCS      = connectionPointCS,
                WheelDirectionCS         = wheelDirectionCS0,
                WheelAxleCS              = wheelAxleCS,
                SuspensionRestLength     = suspensionRestLength,
                WheelRadius              = wheelRadius,
                SuspensionStiffness      = tuning.SuspensionStiffness,
                WheelsDampingCompression = tuning.SuspensionCompression,
                WheelsDampingRelaxation  = tuning.SuspensionDamping,
                FrictionSlip             = tuning.FrictionSlip,
                IsFrontWheel             = isFrontWheel,
                MaxSuspensionTravelCm    = tuning.MaxSuspensionTravelCm,
                MaxSuspensionForce       = tuning.MaxSuspensionForce
            };

            Array.Resize(ref wheelInfo, wheelInfo.Length + 1);
            RobotWheelInfo wheel = new RobotWheelInfo(ci);

            wheelInfo[wheelInfo.Length - 1] = wheel;

            UpdateWheelTransformsWS(wheel, false);
            UpdateWheelTransform(NumWheels - 1, false);
            return(wheel);
        }
Exemple #3
0
        /// <summary>
        /// Updates the position of the wheel according to the BRaycastVehicle's position and speed.
        /// </summary>
        private void Update()
        {
            if (robot == null)
            {
                return;
            }

            RobotWheelInfo wheel = robot.RaycastRobot.GetWheelInfo(wheelIndex);

            if (wheel.Brake == 0f)
            {
                wheel.Brake = (RollingFriction / radius) * robot.RaycastRobot.OverrideMass * MassTorqueScalar;
            }

            transform.position       = wheel.WorldTransform.Origin.ToUnity();
            transform.localRotation *= Quaternion.AngleAxis(-wheel.Speed, axis);
        }
Exemple #4
0
        /// <summary>
        /// Updates the friction for the RaycastRobot.
        /// </summary>
        /// <param name="timeStep"></param>
        public void UpdateFriction(float timeStep)
        {
            //calculate the impulse, so that the wheels don't move sidewards
            int numWheel = NumWheels;

            if (numWheel == 0)
            {
                return;
            }

            Array.Resize <Vector3>(ref forwardWS, numWheel);
            Array.Resize <Vector3>(ref axle, numWheel);
            Array.Resize <float>(ref forwardImpulse, numWheel);
            Array.Resize <float>(ref sideImpulse, numWheel);

            int numWheelsOnGround = 0;

            //collapse all those loops into one!
            for (int i = 0; i < NumWheels; i++)
            {
                RigidBody groundObject = wheelInfo[i].RaycastInfo.GroundObject as RigidBody;
                if (groundObject != null)
                {
                    numWheelsOnGround++;
                }
                sideImpulse[i]    = 0;
                forwardImpulse[i] = 0;
            }

            for (int i = 0; i < NumWheels; i++)
            {
                RobotWheelInfo wheel = wheelInfo[i];

                RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;
                if (groundObject != null)
                {
                    Matrix wheelTrans = GetWheelTransformWS(i);

                    axle[i] = new Vector3(
                        wheelTrans[0, indexRightAxis],
                        wheelTrans[1, indexRightAxis],
                        wheelTrans[2, indexRightAxis]);

                    Vector3 surfNormalWS = wheel.RaycastInfo.ContactNormalWS;
                    float   proj;
                    Vector3.Dot(ref axle[i], ref surfNormalWS, out proj);
                    axle[i] -= surfNormalWS * proj;
                    axle[i].Normalize();

                    Vector3.Cross(ref surfNormalWS, ref axle[i], out forwardWS[i]);
                    forwardWS[i].Normalize();

                    ResolveSingleBilateral(RootRigidBody, wheel.RaycastInfo.ContactPointWS,
                                           groundObject, wheel.RaycastInfo.ContactPointWS,
                                           0, axle[i], ref sideImpulse[i], timeStep);

                    sideImpulse[i] *= wheel.SlidingFriction;
                }
                else
                {
                    if (wheel.Speed > 0)
                    {
                        wheel.Speed = Math.Max(wheel.Speed - wheel.FreeSpinDamping, 0f);
                    }
                    else if (wheel.Speed < 0)
                    {
                        wheel.Speed = Math.Min(wheel.Speed + wheel.FreeSpinDamping, 0f);
                    }
                }
            }

            bool sliding = false;

            for (int i = 0; i < NumWheels; i++)
            {
                RobotWheelInfo wheel        = wheelInfo[i];
                RigidBody      groundObject = wheel.RaycastInfo.GroundObject as RigidBody;

                float rollingFriction = 0.0f;

                if (groundObject != null)
                {
                    Vector3 velocity      = chassisBody.GetVelocityInLocalPoint(wheel.ChassisConnectionPointCS);
                    Vector3 localVelocity = Vector3.TransformNormal(velocity, Matrix.Invert(chassisBody.WorldTransform.Basis));
                    Vector3 forwardAxis   = (UnityEngine.Quaternion.AngleAxis(90f, UnityEngine.Vector3.up) *
                                             wheel.WheelAxleCS.ToUnity() / (MathUtil.SIMD_PI * wheel.WheelsRadius)).ToBullet();

                    float speed = Vector3.Dot(localVelocity, forwardAxis);

                    wheel.Speed = speed;

                    if (wheel.EngineForce != 0.0f)
                    {
                        //apply torque curves
                        float engineForce = wheel.EngineForce;

                        if (speed * engineForce > 0)
                        {
                            engineForce *= 1 - (Math.Abs(speed) / MaxWheelAngularVelocity);
                        }

                        rollingFriction = engineForce * timeStep;

                        if (!RootRigidBody.IsActive)
                        {
                            RootRigidBody.Activate();
                        }
                    }
                    else
                    {
                        float defaultRollingFrictionImpulse = 0.0f;
                        float maxImpulse = (wheel.Brake != 0) ? wheel.Brake : defaultRollingFrictionImpulse;
                        rollingFriction = CalcRollingFriction(RootRigidBody, groundObject, wheel.RaycastInfo.ContactPointWS, forwardWS[i], maxImpulse);
                    }
                }

                //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)

                forwardImpulse[i]     = 0;
                wheelInfo[i].SkidInfo = 1.0f;

                if (groundObject != null)
                {
                    wheelInfo[i].SkidInfo = 1.0f;

                    float maximp     = wheel.WheelsSuspensionForce * timeStep * wheel.FrictionSlip;
                    float maximpSide = maximp;

                    float maximpSquared = maximp * maximpSide;

                    forwardImpulse[i] = rollingFriction;

                    float x = forwardImpulse[i] * fwdFactor;
                    float y = sideImpulse[i] * sideFactor;

                    float impulseSquared = (x * x + y * y);

                    if (impulseSquared > maximpSquared)
                    {
                        sliding = true;

                        float factor = maximp / (float)System.Math.Sqrt(impulseSquared);

                        wheelInfo[i].SkidInfo *= factor;
                    }
                }
            }

            if (sliding)
            {
                for (int wheel = 0; wheel < NumWheels; wheel++)
                {
                    if (sideImpulse[wheel] != 0)
                    {
                        if (wheelInfo[wheel].SkidInfo < 1.0f)
                        {
                            forwardImpulse[wheel] *= wheelInfo[wheel].SkidInfo;
                            sideImpulse[wheel]    *= wheelInfo[wheel].SkidInfo;
                        }
                    }
                }
            }

            // apply the impulses
            for (int i = 0; i < NumWheels; i++)
            {
                WheelInfo wheel = wheelInfo[i];

                Vector3 rel_pos = wheel.RaycastInfo.ContactPointWS -
                                  chassisBody.CenterOfMassPosition;

                if (forwardImpulse[i] != 0)
                {
                    chassisBody.ApplyImpulse(forwardWS[i] * forwardImpulse[i], rel_pos);
                }
                if (sideImpulse[i] != 0)
                {
                    RigidBody groundObject = wheel.RaycastInfo.GroundObject as RigidBody;

                    Vector3 rel_pos2 = wheel.RaycastInfo.ContactPointWS -
                                       groundObject.CenterOfMassPosition;


                    Vector3 sideImp = axle[i] * sideImpulse[i];

#if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
                    //Vector4 vChassisWorldUp = RigidBody.CenterOfMassTransform.get_Columns(indexUpAxis);
                    Vector3 vChassisWorldUp = new Vector3(
                        RigidBody.CenterOfMassTransform.Row1[indexUpAxis],
                        RigidBody.CenterOfMassTransform.Row2[indexUpAxis],
                        RigidBody.CenterOfMassTransform.Row3[indexUpAxis]);
                    float dot;
                    Vector3.Dot(ref vChassisWorldUp, ref rel_pos, out dot);
                    rel_pos -= vChassisWorldUp * (dot * (1.0f - wheel.RollInfluence));
#else
                    rel_pos[indexUpAxis] *= wheel.RollInfluence;
#endif
                    chassisBody.ApplyImpulse(sideImp, rel_pos);

                    //apply friction impulse on the ground
                    groundObject.ApplyImpulse(-sideImp, rel_pos2);
                }
            }
        }