void Start()
{
//mSpeed=5;
if (!CelluloInit)
{
Cellulo.initialize();
CelluloInit = true;
print("Cellulo initialized");
}
if (robot != null)
{
//robot.clearTracking();
start_time = Time.time;
MappingMode = 0;
convertCoord = this.gameObject.GetComponent <LibDotsMapping>();
//isRunning=true;
MasterInfo = GameObject.Find("SimMasterInfo");
TestGO = GameObject.Find("Test GO");
}
}
void Update()
{
if (MasterInfo == null)
{
MasterInfo = GameObject.Find("SimMasterInfo");
CelluloTrakingScript = MasterInfo.GetComponent <CelluloTracker>();
}
if (TestGO == null)
{
TestGO = GameObject.Find("Test GO");
}
//rotSpeed=globalFlock.AutorotationSpeed;
amIcontrolledUpdate();
celluloTheta = 0f;
celluloInput = Vector3.zero;
if (robot != null)
{
currentRobotPos.Set(robot.getX(), robot.getY(), 10.0f);
celluloTheta = robot.getTheta() - initialCelluloTheta; //get the orientation of the robot - onConnect orientation
// Debug.Log("Pos value for Cellulo :"+robot.getID()+" is :"+currentRobotPos);
float now = Time.time;
convertCoord = this.gameObject.GetComponent <LibDotsMapping>();
celluloInput = convertCoord.libDotsConvertion(currentRobotPos[0], currentRobotPos[1], (int)MappingMode);
DebugInputLog = celluloInput;
DebugInputLog[2] = celluloTheta;
robotVisualUpdate();
GizmoOn = false;
}
else
{
k++;
if (k > 1) //soften load on Unity Editor; not needed anymore
{
k = 0;
if (CelluloTrakingScript.unAssociatedRobotsCount() >= 1)
{
connect();
}
else
{
;
//Debug.Log("Remaining robots < 1"+ "\n Robots in pool ="+Cellulo.totalRobots());
}
}
}
GameObject cam3p = GameObject.Find("3rdPersonCamera");
Transform camTransform = cam3p.transform;
Vector3 CamXAngleCorrection = Vector3.zero;
CamXAngleCorrection.Set(-camTransform.eulerAngles[0], 0, 0);
camTransform.Rotate(CamXAngleCorrection);
//transform.Translate(mSpeed*Input.GetAxis("Horizontal")*Time.deltaTime,0,mSpeed*Input.GetAxis("Depth")*Time.deltaTime, relativeTo);
//transform.Translate(mSpeed*Input.GetAxis("Horizontal")*Time.deltaTime,mSpeed*Input.GetAxis("Vertical")*Time.deltaTime,mSpeed*Input.GetAxis("Depth")*Time.deltaTime, camTransform);
//Cellulo input controls
if (celluloLessDebug)
{
celluloInput[0] = debugCelluloX;
celluloInput[1] = debugCelluloY;
celluloTheta = debugCelluloTheta;
}
if (robot != null)
{
//If controlled by cellulo ; Filter
MasterInfo.GetComponent <GameScript2>().sliderX.value = celluloInput[0];
MasterInfo.GetComponent <GameScript2>().sliderY.value = celluloInput[1];
}
//Z Orientation Correction
realignZOrientation();
if (MoveMode == ControlMode.orientationControl && MappingMode == Mode.Mono)
{
//Intuitive Joystick Input Swap
celluloInput = swapInput(celluloInput); //Rotations around X axis means changes in YZ plane and Z=0 so in Y direction
//Rotation modifier for turning faster in horizontal plane
celluloInput[1] *= horizontalToVerticalRotationAssistRatio;
celluloInput[0] = -celluloInput[0]; //positive X is down (left handed axis in Unity)
//If Idle in X rotation, slowly go back to horizontal plane movement
int XrotInput = 0;
if (Mathf.Abs(celluloInput[XrotInput]) < 0.2f && transform.eulerAngles[0] > 0.1)
{
celluloInput[XrotInput] = Mathf.Sign(transform.eulerAngles[0] - 180) * 0.45f; // Rotation around X (side) back to 0 slowly if idle
if (transform.eulerAngles[0] < 1)
{
transform.eulerAngles = new Vector3(0, transform.eulerAngles[1], transform.eulerAngles[2]);
}
}
//rotation applied to individual in swarm from cellulo input (0 if no cellulo and no debug active)
float rotCoefToApply = (robot != null || celluloLessDebug)?globalFlock.CelluloRotationSpeed:globalFlock.AutorotationSpeed;
this.transform.Rotate(celluloInput * rotCoefToApply * Time.deltaTime);
transform.eulerAngles = new Vector3(ClampAngle(transform.eulerAngles[0], -60, 60), transform.eulerAngles[1], transform.eulerAngles[2]); //clamp rotation to [-90;90]
if (robot != null || celluloLessDebug)
{
this.GetComponent <IndiFlock>().CelluoInputQuat = this.transform.rotation;
}
}
//SpeedPhiTheta
if (MoveMode == ControlMode.SpeedPhiTheta && MappingMode == Mode.Mono)
{
/*Controlls are : X : speed variation : max at 0 (velocity control)
* Y : Create Rotation around X (side) axis (angular velocity control)
* Theta : Set fish Theta (angle control)
*/
this.gameObject.GetComponent <IndiFlock>().forwardSpeedModfier = 1f - (0.9f * Mathf.Abs(celluloInput[0]));
celluloInput[1] *= -1; //Inverse Y controlls
//On Low Cellulo[1], restore rotX to 0 slowly
//ToDO : modify : does not go back slowly if Y angle >90° tested : errors in this mode
int XrotInput = 1;
/* if (Mathf.Abs(celluloInput[XrotInput])<0.3f && transform.eulerAngles[0]>0.1)
* {
* int sign=(ClampAngle(transform.eulerAngles[0],-60,60)>180)?1:-1;
*
* //Modify this :
* celluloInput[XrotInput]=-sign*0.45f; // Rotation around X (side) back to 0 slowly if idle
* if(amIcontrolled){
* print(transform.eulerAngles[0]+ " DEBUG\n " +sign);
* }
* if (transform.eulerAngles[0]<1)
* transform.eulerAngles=new Vector3(0, transform.eulerAngles[1], transform.eulerAngles[2]);
* } */
Vector3 Xrotation = Vector3.zero;
float sensitivity = 30f;
Xrotation[0] = celluloInput[1] * sensitivity;
Quaternion XQuat = Quaternion.identity;
XQuat = Quaternion.AngleAxis(celluloInput[1] * sensitivity, Vector3.right);
XQuat.SetLookRotation(XQuat * Vector3.forward);
Quaternion celluloThetaRot = Quaternion.Euler(new Vector3(0, celluloTheta, 0));
if (amIcontrolled)
{
Debug.DrawLine(this.transform.position, this.transform.position + celluloThetaRot * Vector3.forward * 5f, Color.green);
}
if (robot != null || celluloLessDebug)
{
this.GetComponent <IndiFlock>().CelluoInputQuat = celluloThetaRot * XQuat;
}
}
if (MoveMode == ControlMode.ThetaSpeedPhi && amIcontrolled)
{
//Y axis is speed control
//celluloInput[1]*=-1;
this.gameObject.GetComponent <IndiFlock>().forwardSpeedModfier = (1f + celluloInput[1]) / 2;
//X axis : Control of theta ; Rotation around the Y axis
Vector3 Yrotation = Vector3.zero;
float sensitivity = 1f; //speed control not position ; sensitivity needed much lower
Yrotation[0] = celluloInput[0] * sensitivity;
Quaternion YQuat = Quaternion.identity;
YQuat = this.transform.rotation;
YQuat *= Quaternion.AngleAxis(celluloInput[0] * sensitivity, Vector3.up); //Rotate around Up Axis
Quaternion YQuatOG = TestGO.transform.rotation * Quaternion.AngleAxis(celluloInput[0] * sensitivity, Vector3.up);
YQuat.SetLookRotation(YQuat * Vector3.forward);
//Theta : controll altitude : on angle changes
float AngleDiff = ShortestAngleDistance(prevTheta, celluloTheta);
if (Mathf.Abs(AngleDiff) < 2f)
{
AngleDiff = 0;
}
StartCoroutine(PrevThetaDelayer(1.5f, celluloTheta));
AngleDiff = Mathf.Clamp(AngleDiff, -120, 120);
Quaternion XQuat = Quaternion.AngleAxis(AngleDiff * 0.5f * sensitivity, Vector3.ProjectOnPlane(this.transform.right, Vector3.up));//TestGO.transform.right);
//if(amIcontrolled) Debug.DrawLine(TestGO.transform.position,TestGO.transform.position+XQuat2*TestGO.transform.forward*8f,Color.white);
//if(amIcontrolled) TestGO.transform.Rotate(0,1,0);
/* if (AngleDiff==0)
* XQuat.SetLookRotation(Vector3.ProjectOnPlane(TestGO.transform.forward,Vector3.up)); */
Quaternion QuatTotal = Quaternion.identity;
Quaternion QuatYrotation = Quaternion.AngleAxis(celluloInput[0] * sensitivity, Vector3.up) * this.transform.rotation;
Transform Test = this.transform;
Test.rotation = QuatYrotation;
QuatTotal = Quaternion.AngleAxis(AngleDiff * 0.5f * sensitivity, Vector3.ProjectOnPlane(Test.right, Vector3.up));
QuatTotal.SetLookRotation(QuatTotal * Vector3.ProjectOnPlane(this.transform.forward, Vector3.up));
Debug.DrawLine(TestGO.transform.position + Vector3.down * 5f, TestGO.transform.position + Vector3.down * 5f + QuatTotal * TestGO.transform.forward * 8f, Color.white);
Debug.DrawLine(this.transform.position, this.transform.position + XQuat * Vector3.forward * 5f, Color.green);
if (robot != null || celluloLessDebug)
{
this.GetComponent <IndiFlock>().CelluoInputQuat = QuatTotal;
}
}
void realignZOrientation()
{
if (Mathf.Abs(transform.eulerAngles[2]) > 0.5)
{
celluloInput[2] = Mathf.Sign(transform.eulerAngles[2] - 180) * Mathf.Max(10f, 1f / (0.001f + 0.0002f * Mathf.Abs(transform.eulerAngles[2] - 180)));
}
else
{
transform.eulerAngles = new Vector3(transform.eulerAngles[0], transform.eulerAngles[1], 0);
}
}
}
