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