///Convert a position by applying sensor deformation.
///@param pos position to convert
///@param weight 0->1
public Vector3 TransformPosition(Vector3 pos, float weight)
{
float dist = Vector3.Distance(pos, m_center);
Vector3 torqueDir;
Vector3 centripetalDir;
Vector3 motionDir = Vector3.zero;
float scaleFactor = VertExmotionBase.GetScaleFactor(transform);
if (dist < m_envelopRadius * scaleFactor)
{
torqueDir = Vector3.Cross((pos - m_center).normalized, m_torqueAxis);
torqueDir *= m_motionTorqueForce * dist;
centripetalDir = (pos - m_center).normalized * m_centripetalForce * .89f;
motionDir = (m_motionDirection + torqueDir + centripetalDir) * (1.0f - dist / (m_envelopRadius * scaleFactor + .0000001f));
}
return(pos + motionDir * weight);
}
public void UpdateCollisionZone(CollisionZone cz)
{
//return;
Vector3 hitNormal = Vector3.zero;
float hitDist = 0;
int count = 0;
float radius = cz.radius * VertExmotionBase.GetScaleFactor(transform);
Vector3 collisionCenter = transform.TransformPoint(cz.positionOffset);// transform.position + transform.rotation * cz.positionOffset;
cz.collisionVector = Vector3.zero;
if (Physics.CheckSphere(collisionCenter, radius, m_layerMask.value))
{
cz.collisionVector = Vector3.zero;
// //test
// Collider[] hitColliders = Physics.OverlapSphere(collisionCenter, m_collisionZones[0].radius, m_layerMask);
// int j = 0;
// while (j < hitColliders.Length)
// {
// Debug.Log("collide " + hitColliders[j].name);
// hitColliders[j].ClosestPointOnBounds( transform.position );
// j++;
// }
for (int a = 0; a < 6; ++a)//WIP : check collision on each axis
//int a = 0;
{
Vector3 dir = Vector3.zero;
switch (a)
{
case 0: dir = m_sensor.transform.forward; break;
case 1: dir = -m_sensor.transform.forward; break;
case 2: dir = m_sensor.transform.up; break;
case 3: dir = -m_sensor.transform.up; break;
case 4: dir = m_sensor.transform.right; break;
case 5: dir = -m_sensor.transform.right; break;
}
//cz.hits = Physics.SphereCastAll(collisionCenter - dir * radius / 2f, radius * m_collisionScaleFactor, dir, radius / 2f, m_layerMask.value, QueryTriggerInteraction.Collide);
cz.hits = Physics.SphereCastAll(collisionCenter - dir * radius, radius * m_collisionScaleFactor, dir, radius, m_layerMask.value, QueryTriggerInteraction.Collide);
for (int i = 0; i < cz.hits.Length; ++i)
{
//Debug.Log("collision "+Vector3.Distance (cz.hits [i].point, collisionCenter));
//Debug.DrawLine(cz.hits[i].collider.ClosestPointOnBounds(transform.position), transform.position);
if (m_ignoreColliders.Contains(cz.hits[i].collider))
{
continue;
}
Vector3 hitPos = cz.hits[i].point;
float debugoffset = .01f;
#if KVTM_DEBUG
Debug.DrawLine(cz.hits[i].collider.ClosestPointOnBounds(transform.position) + Vector3.up * a * debugoffset, transform.position + Vector3.up * a * debugoffset, Color.blue);
#endif
//Vector3 hitPos = cz.hits [i].collider.ClosestPointOnBounds(transform.position);
//if (Vector3.Distance (cz.hits [i].point, collisionCenter) < radius )
if (Vector3.Distance(hitPos, collisionCenter) < radius)
{
/*
* hitNormal = cz.hits[i].normal.normalized;
* //hitNormal += (collisionCenter-cz.hits [i].point ).normalized;
* hitNormal += (collisionCenter - hitPos).normalized;
* hitNormal.Normalize();
*/
hitNormal = -(hitPos - collisionCenter).normalized;
//if (Vector3.Dot ((transform.position - hits [i].point).normalized, hits [i].normal) >= 0)
if (Vector3.Dot((collisionCenter - hitPos).normalized, -dir) > 0)
{
//hitDist += (radius - Vector3.Distance(hitPos, collisionCenter));
hitDist += (radius - Vector3.Distance(hitPos, collisionCenter)) / 6f;
count++;
} /*
* else
* {
* if (cz.hits[i].collider as SphereCollider == null)
* {
* hitNormal = (-dir + cz.hits[i].normal).normalized;
* hitDist = radius;
* }
* else
* {
* hitNormal = cz.hits[i].normal.normalized;
* hitDist = radius - Vector3.Distance(hitPos, collisionCenter);
* }
* }*/
//count++;
} /*
* else
* {
* hitDist = cz.radius;
* hitNormal = (-m_sensor.transform.forward + cz.hits[i].normal).normalized;
* }*/
}
if (count > 0)
{
cz.collisionVector += hitNormal.normalized * hitDist * 2f;
}/*
* else
* {
* cz.collisionVector += hitNormal.normalized * radius * 2f;
* }*/
}
//cz.collisionVector /= 6f;
}
else
{
cz.collisionVector = Vector3.zero;
}
}
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;
}
public static bool DrawSensorHandle(VertExmotionSensorBase sensor)
{
bool useHandle = false;
Color handleColor = VertExmotionEditor.orange;
Handles.color = handleColor;
Camera svCam = SceneView.currentDrawingSceneView.camera;
float constUnit = (svCam.ViewportToWorldPoint(Vector3.zero) - svCam.ViewportToWorldPoint(Vector3.one)).magnitude;
constUnit = HandleUtility.GetHandleSize(sensor.transform.position) * 10f;
if (m_settingMode == eSettingsMode.SENSORS)
{
Handles.DrawSolidDisc(sensor.transform.position, -svCam.transform.forward, (constUnit * .01f));
Handles.DrawWireDisc(sensor.transform.position, -svCam.transform.forward, sensor.m_envelopRadius * VertExmotionBase.GetScaleFactor(sensor.transform));
for (int i = 0; i < 10; ++i)
{
handleColor.a = (float)(10 - i) / 10f * .5f;
float f = (float)i / 11f * (float)i / 11f;
Handles.color = handleColor;
Handles.DrawWireDisc(sensor.transform.position, -svCam.transform.forward, sensor.m_envelopRadius * VertExmotionBase.GetScaleFactor(sensor.transform) * f);
}
handleColor = VertExmotionEditor.orange;
Handles.color = handleColor;
Vector3 lastPos = sensor.transform.position;
sensor.transform.position =
Handles.FreeMoveHandle(sensor.transform.position, Quaternion.identity, (constUnit * .02f), Vector3.zero, Handles.CircleCap);
if (lastPos != sensor.transform.position)
{
useHandle = true;
}
float lastRadius = sensor.m_envelopRadius;
if (VertExmotionBase.GetScaleFactor(sensor.transform) > 0)
{
sensor.m_envelopRadius =
Vector3.Distance(
Handles.FreeMoveHandle(sensor.transform.position + svCam.transform.right * sensor.m_envelopRadius * VertExmotionBase.GetScaleFactor(sensor.transform), Quaternion.identity, (constUnit * .02f), Vector3.zero, Handles.CubeCap)
, sensor.transform.position) / VertExmotionBase.GetScaleFactor(sensor.transform);
}
if (lastRadius != sensor.m_envelopRadius)
{
useHandle = true;
}
Handles.color = Color.cyan;
//draw direction
lastPos = sensor.transform.position + sensor.transform.forward * constUnit * .1f;
Handles.DrawLine(sensor.transform.position, lastPos);
lastPos = Handles.FreeMoveHandle(lastPos, Quaternion.identity, (constUnit * .01f), Vector3.zero, Handles.CircleCap);
if (lastPos != sensor.transform.position + sensor.transform.forward * constUnit * .1f)
{
sensor.transform.LookAt(lastPos);
useHandle = true;
}
//--------------------------------------------------
//draw sensors limits
//--------------------------------------------------
Color col = Color.blue;
col.a = .01f;
Handles.color = col;
Vector3[] limitAxis = new Vector3[4];
limitAxis [0] = sensor.transform.right;
limitAxis [1] = -sensor.transform.right;
limitAxis [2] = sensor.transform.up;
limitAxis [3] = -sensor.transform.up;
float sf = VertExmotionBase.GetScaleFactor(sensor.transform);
for (int n = 0; n < 4; n++)
{
col.a = .3f;
Handles.color = col;
float max = 20f;
Vector3[] points = new Vector3[(int)max + 1];
for (float i = 0; i <= max; ++i)
{
Vector3 p1 = Quaternion.AngleAxis(i / max * 180f, limitAxis[n]) * sensor.transform.forward * Mathf.Max(sensor.m_params.translation.innerMaxDistance, sensor.m_params.translation.outerMaxDistance) * sf;
float lerpFactor = (Vector3.Dot(sensor.transform.forward, p1.normalized) + 1f) * .5f;
float clampMag = (Mathf.Lerp(sensor.m_params.translation.innerMaxDistance, sensor.m_params.translation.outerMaxDistance, lerpFactor) * sf);
p1 = Vector3.ClampMagnitude(p1, clampMag);
p1 += sensor.transform.position;
if (i % 4 == 0)
{
Handles.DrawDottedLine(sensor.transform.position, p1, 3f);
}
points[(int)i] = p1;
}
col.a = .5f;
Handles.color = col;
Handles.DrawPolyLine(points);
}
col.a = .03f;
Handles.color = col;
Handles.DrawSolidDisc(sensor.transform.position, sensor.transform.forward, (sensor.m_params.translation.innerMaxDistance + sensor.m_params.translation.outerMaxDistance) * .5f * sf);
col.a = .5f;
Handles.color = col;
Handles.DrawWireDisc(sensor.transform.position, sensor.transform.forward, (sensor.m_params.translation.innerMaxDistance + sensor.m_params.translation.outerMaxDistance) * .5f * sf);
}
//--------------------------------------------------
//draw collider handles
//--------------------------------------------------
if (m_settingMode == eSettingsMode.COLLIDERS)
{
VertExmotionColliderBase vtmCol = sensor.GetComponentInChildren <VertExmotionColliderBase>();
Handles.color = Color.cyan;
if (vtmCol != null)
{
for (int i = 0; i < vtmCol.m_collisionZones.Count; ++i)
{
Vector3 worldColZonePos = vtmCol.transform.TransformPoint(vtmCol.m_collisionZones[i].positionOffset);
float radius = vtmCol.m_collisionZones[i].radius * VertExmotionBase.GetScaleFactor(vtmCol.transform);
//Handles.DrawSolidDisc( vtmCol.m_collisionZones[i].positionOffset + sensor.transform.position , -svCam.transform.forward, ( constUnit*.01f ) );
Handles.color = m_collisionZoneAlpha;
Handles.DrawSolidDisc(worldColZonePos, -svCam.transform.forward, radius);
Handles.color = m_collisionZone;
Handles.DrawWireDisc(worldColZonePos, sensor.transform.forward, radius);
Handles.DrawWireDisc(worldColZonePos, sensor.transform.up, radius);
Handles.DrawWireDisc(worldColZonePos, sensor.transform.right, radius);
Handles.DrawSolidDisc(worldColZonePos, -svCam.transform.forward, (constUnit * .01f));
if (VertExmotionBase.GetScaleFactor(vtmCol.transform) > 0)
{
vtmCol.m_collisionZones[i].radius =
Vector3.Distance(
Handles.FreeMoveHandle(worldColZonePos + svCam.transform.right * radius, Quaternion.identity, (constUnit * .02f), Vector3.zero, Handles.CubeCap)
, worldColZonePos) / VertExmotionBase.GetScaleFactor(vtmCol.transform);
}
vtmCol.m_collisionZones[i].positionOffset = vtmCol.transform.InverseTransformPoint(Handles.FreeMoveHandle(worldColZonePos, Quaternion.identity, (constUnit * .02f), Vector3.zero, Handles.CircleCap));
}
}
}
if (useHandle)
{
EditorUtility.SetDirty(sensor);
}
return(useHandle);
}
