/// <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); }
/// <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, rootNode.WheelsNormal.ToBullet(), axle, suspensionRestLength, radius, defaultVehicleTuning, false); wheel.RollInfluence = RollInfluence; wheel.SlidingFriction = slidingFriction; return(RaycastRobot.NumWheels - 1); }
/// <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; } if (updatePosition) { transform.position = wheel.WorldTransform.Origin.ToUnity(); } transform.localRotation *= Quaternion.AngleAxis(-wheel.Speed, axis); }
/// <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, RightAxis], wheelTrans[1, RightAxis], wheelTrans[2, RightAxis]); 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 = RigidBody.GetVelocityInLocalPoint(wheel.ChassisConnectionPointCS); Vector3 localVelocity = Vector3.TransformNormal(velocity, Matrix.Invert(RigidBody.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 - RigidBody.CenterOfMassPosition; if (forwardImpulse[i] != 0) { RigidBody.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 RigidBody.ApplyImpulse(sideImp, rel_pos); //apply friction impulse on the ground groundObject.ApplyImpulse(-sideImp, rel_pos2); } } }
/// <summary> /// Get the wheel speed to be used for encoder calculations /// </summary> /// <returns></returns> public float GetWheelSpeed() { RobotWheelInfo wheel = robot.RaycastRobot.GetWheelInfo(wheelIndex); return(wheel.Speed); }