void Update() { float newAngle = Quaternion.Angle(m_rotationSensor.transform.rotation, m_lastRotation); Vector3 c1 = Vector3.Cross(m_lastUp, m_rotationSensor.transform.up); newAngle *= Vector3.Dot(c1.normalized, m_rotationSensor.transform.forward) > 0f ? 1f : -1f; anglePid.m_target = -newAngle * m_rotationFactor; anglePid.m_params.kp = m_rotationDamping; anglePid.m_params.ki = m_rotationBouncing; anglePid.m_params.limits.x = -m_maxAngle; anglePid.m_params.limits.y = m_maxAngle; m_angle = anglePid.Compute(0f); inflatePid.m_target = Mathf.Abs(newAngle) * m_rotationToInflate; inflatePid.m_params.kp = m_inflateDamping; inflatePid.m_params.ki = m_inflateBouncing; inflatePid.m_params.limits.x = 0; inflatePid.m_params.limits.y = 1; m_inflate = inflatePid.Compute(0f); m_rotationSensor.m_params.rotation.angle = m_angle; m_rotationSensor.m_params.inflate = m_inflate; m_lastRotation = m_rotationSensor.transform.rotation; m_lastUp = m_rotationSensor.transform.up; }
public static void GUIDrawPidResponse(PID pid, Rect area, float timeUnit) { Color c = new Color(1f, 1f, 1f, .1f); pid.Init(); //unit step pid.m_target = 1; float r = 0; Vector2 start = new Vector2(area.x, area.y + area.height); Vector2 end = start; Handles.color = c; for (int i = 0; i < timeUnit; ++i) { start = new Vector2((float)i * area.width / timeUnit, area.y + area.height); end = new Vector2((float)i * area.width / timeUnit, area.y); //GLDraw.DrawLine (start, end, c, 1f); Handles.DrawLine(start, end); } start = new Vector2(area.x, area.y + area.height * .5f); end = new Vector2(area.x + area.width, area.y + area.height * .5f); //GLDraw.DrawLine (start, end, c, 1f); Handles.DrawLine(start, end); start = new Vector2(area.x, area.y + area.height); end = start; for (int i = 0; i < area.width; ++i) { float dt = (float)timeUnit / (float)area.width; for (int j = 0; j < 10f; ++j) { r = pid.Compute(r, dt * .1f); } end.x++; end.y = area.height - r * area.height * .5f + area.y; end.y = Mathf.Clamp(end.y, area.y, area.y + area.height); //GLDraw.DrawLine (start, end, Color.green, 1f); Handles.color = VertExmotionEditor.orange; Handles.DrawLine(start, end); start = end; // //draw error // errEnd.x++; // errEnd.y = area.height - (float) lastErr; // errEnd.y = Mathf.Clamp( errEnd.y, area.y, area.y+area.height ); // GLDraw.DrawLine (errStart, errEnd, Color.red, 1f); // errStart = errEnd; } }
public Vector3 Compute(Vector3 input) { m_pidX.m_params = m_params; m_pidX.m_target = m_target.x; m_pidY.m_params = m_params; m_pidY.m_target = m_target.y; m_pidZ.m_params = m_params; m_pidZ.m_target = m_target.z; Vector3 r = Vector3.zero; r.x = m_pidX.Compute(input.x); r.y = m_pidY.Compute(input.y); r.z = m_pidZ.Compute(input.z); return(r); }
void Update() { float sf = VertExmotionBase.GetScaleFactor(transform); m_center = transform.position; m_pid.m_params.kp = m_params.damping; m_pid.m_params.ki = m_params.bouncing; /* * if (Time.deltaTime > 0) * m_pid.m_target = (m_lastPosition - transform.position) * m_params.translation.amplitudeMultiplier / Time.deltaTime * .1f; */ if (Time.deltaTime * m_pid.m_pidX.m_timeScale > 0) { m_speed = (m_lastPosition - transform.position) / (Time.deltaTime * m_pid.m_pidX.m_timeScale); } //Debug.Log ("speed " + m_speed.y); /* * if ( Time.deltaTime * m_pid.m_pidX.m_timeScale > 0) * m_accSquash = Vector3.Lerp ( m_accSquash, ( m_lastSpeed - m_speed )/ ( Time.deltaTime * m_pid.m_pidX.m_timeScale ), Time.deltaTime * 10f );; */ //Debug.Log ("squash min " + m_speed.magnitude ); if (m_speed.magnitude > m_params.fx.stretchMinSpeed) { m_speedStrech = Vector3.Lerp(m_speedStrech, m_speed.normalized * (m_speed.magnitude - m_params.fx.stretchMinSpeed), Time.deltaTime * 2f * timeScale); } else { m_speedStrech = Vector3.Lerp(m_speedStrech, Vector3.zero, Time.deltaTime * 2f * timeScale); } m_speedStrech = Vector3.ClampMagnitude(m_speedStrech, m_params.fx.stretchMax); if (Vector3.Dot(m_speed.normalized, m_lastSpeed.normalized) > 0) { m_stretch = Mathf.Lerp(m_stretch, m_params.fx.stretch, Time.deltaTime * 10f * timeScale); } else { m_stretch = Mathf.Lerp(m_stretch, 0, Time.deltaTime * 1f * timeScale); } //m_speedSquash = m_accSquash; //m_speedSquash = -transform.parent.rigidbody.velocity; m_pid.m_target = m_speed * m_params.translation.amplitudeMultiplier * .1f; //m_pid.m_target = Vector3.up * Mathf.Sin (Time.time) * .5f; //Debug.Log ( "target : " + m_pid.m_target ); //compute translation float lerpFactor = (Vector3.Dot(transform.forward, m_sensorDirection.normalized) + 1f) * .5f; //sensor limits float clampMag = (Mathf.Lerp(m_params.translation.innerMaxDistance, m_params.translation.outerMaxDistance, lerpFactor)) * sf; m_pid.m_params.limits.x = -clampMag; m_pid.m_params.limits.y = clampMag; //compute sensor position m_sensorDirection = m_pid.Compute(m_sensorDirection); //---------------------------------------------------------------------------- //compute torque Force (need more work...) m_torqueAxis = Vector3.zero; m_motionTorqueForce = 0; //m_motionTorqueForce = Mathf.Lerp (m_motionTorqueForce, ( 1f - Quaternion.Dot (transform.rotation, m_lastRotation) ) *m_torqueAmplitude, Time.time * m_torqueSpeed); float angleSpeed = 0f; // = (1f - Quaternion.Dot (transform.rotation, m_lastRotation)) * m_params.torque.amplitude;// + m_motionTorqueForce; //angle = ( Quaternion.Angle (transform.rotation, m_lastRotation) ); if (Time.deltaTime > 0) { angleSpeed = Vector3.Angle(m_lastRight, transform.right) / Time.deltaTime; } //m_angleCumul += angle; //Debug.Log ( angle ); m_torqueForcePID.m_params.kp = m_params.torque.damping; m_torqueForcePID.m_params.ki = m_params.torque.bouncing; //newTorque = Mathf.Lerp (newTorque, m_motionTorqueForce, Time.time * m_params.torque.smooth); //m_torqueForcePID.m_target = Mathf.Lerp ( m_torqueForcePID.m_target, angle * Time.deltaTime * m_params.torque.amplitude, Time.deltaTime * m_params.torque.smooth ); //m_torqueForcePID.m_target = Mathf.Clamp (m_torqueForcePID.m_target, -m_params.torque.max, m_params.torque.max); m_torqueForcePID.m_target = angleSpeed * m_params.torque.amplitude; m_torqueForcePID.m_target = Mathf.Clamp(m_torqueForcePID.m_target, -m_params.torque.max, m_params.torque.max); m_motionTorqueForce = m_torqueForcePID.Compute(m_motionTorqueForce); //m_motionTorqueForce += newTorque; m_motionTorqueForce = Mathf.Clamp(m_motionTorqueForce, -m_params.torque.max, m_params.torque.max); Vector3 newAxis = Vector3.Cross(m_lastForward, transform.forward) + Vector3.Cross(m_lastRight, transform.right); newAxis.Normalize(); //move forward on torque //m_sensorDirection += transform.forward * Mathf.Abs( Vector3.Dot (newAxis, transform.forward) ) * m_motionTorqueForce * .1f; //m_torqueAxis = Vector3.Lerp (m_torqueAxis, newAxis, Time.deltaTime * 10f ); m_torqueAxisPID.m_params.kp = m_params.torque.damping; m_torqueAxisPID.m_params.ki = m_params.torque.bouncing; m_torqueAxisPID.m_target = newAxis; m_torqueAxis = m_torqueAxisPID.Compute(m_torqueAxis); //m_torqueAxis = newAxis; m_motionTorqueForce *= Vector3.Dot(m_torqueAxis, transform.forward); //limit torque on Z axis //init for next frame //m_lastRotation = transform.rotation; m_lastForward = transform.forward; m_lastRight = transform.right; //---------------------------------------------------------------------------- m_lastPosition = transform.position; m_lastSpeed = m_speed; if (m_collision.magnitude < .0001f) { m_motionDirection = Vector3.zero; } else { m_sensorDirection = Vector3.Lerp(m_sensorDirection, m_collision, Time.deltaTime); m_motionDirection = m_collision; } m_motionDirection += m_sensorDirection + (m_params.translation.worldOffset + transform.TransformDirection(m_params.translation.localOffset)) * sf; //gravity Vector3 gravityTorqueAxis = Vector3.zero; float gravityTorqueForce = 0; Vector3 gravityDirection = Vector3.zero; if (m_params.translation.gravityInOut.magnitude != 0) { Vector3 g = Physics.gravity.normalized; float dot = Vector3.Dot(g, transform.forward); gravityDirection = g * Mathf.Lerp(m_params.translation.gravityInOut.x, m_params.translation.gravityInOut.y, (dot + 1f) * .5f) * Physics.gravity.magnitude; // * ( Vector3.Dot(g,transform.forward )>0 ? Vector3.Dot(g,transform.forward ) : 0 ); gravityTorqueAxis = Vector3.Cross(g, transform.forward); //m_motionTorqueForce = 1f;// m_params.translation.gravityFactor * 2f; gravityTorqueForce = (1f - dot * dot) * Mathf.Lerp(dot, 1f, dot * dot); //(dot<0?dot:1f); //m_motionTorqueForce = ( 1f - dot*dot );//(dot<0?dot:1f); } /* * else * { * //m_torqueAxis = * m_motionTorqueForce = 0f; * }*/ m_motionDirection += gravityDirection * sf; //m_motionDirection = Vector3.ClampMagnitude (m_motionDirection, clampMag); //Debug.DrawLine (transform.position, transform.position + m_motionDirection.normalized * 10f, Color.red); m_centripetalForce = m_params.inflate; //todo add torque force //Add gravity m_torqueAxis += gravityTorqueAxis; m_motionTorqueForce += gravityTorqueForce; /* * if( m_torqueAxis.magnitude == 0 ) * m_torqueAxis = transform.forward; * else * m_torqueAxis.Normalize();*/ //apply global smooth m_motionDirection = Vector3.Lerp(m_motionDirection, m_lastMotionDirection, Time.deltaTime * m_params.globalSmooth); if (m_parentSensor != null) { m_motionDirection += m_parentSensor.m_motionDirection; } m_lastMotionDirection = m_motionDirection; }