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;
}
