public static Dictionary <string, bool> categoriesExpanded = new Dictionary <string, bool>(); // Category collapsed or expanded
/// <summary>
/// Show the value of this category(Default) - key pair on Lebug Windows.
/// Can use same key names in different categories.
/// Health key in enemy and player category.
/// </summary>
/// <param name="key"></param>
/// <param name="value"></param>
/// <param name="category">Optional, Category name</param>
/// <param name="expanded">Optional, Should the category start expanded or collapsed</param>
public static void Log(string key, object value, string category = "Default", bool expanded = true)
{
if (null == value)
{
value = "NULL";
}
if (null == key)
{
System.Diagnostics.StackTrace stackTrace = new System.Diagnostics.StackTrace();
Lebug.Log("Null Key sent for (" + value + ") from class-method:", stackTrace.GetFrame(1).GetMethod().DeclaringType.Name + " - " + stackTrace.GetFrame(1).GetMethod().Name, "LEBUG ERROR");
return;
}
if (!lebugDict.ContainsKey(category))
{
categoriesExpanded.Add(category, expanded);
lebugDict.Add(category, new Dictionary <string, object>());
}
if (lebugDict[category].ContainsKey(key))
{
lebugDict[category][key] = value.ToString();
}
else
{
lebugDict[category].Add(key, value.ToString());
}
}
void PrintViaLebugLog()
{
Lebug.Log("Rifle Ammo", RifleAmmo, "Ammunition");
Lebug.Log("Rocket Ammo", RocketAmmo, "Ammunition");
Lebug.Log("LevelName", LevelName, "LevelInfo", false); // Start the category collapsed
Lebug.Log("EpisodeNo", EpisodeNo, "LevelInfo", false); // Start the category collapsed
Lebug.Log("MissionNo", MissionNo, "LevelInfo", false); // Start the category collapsed
Lebug.Log("Health", PlayerHealth, "PlayerStats");
Lebug.Log("Armor", PlayerArmor, "PlayerStats");
Lebug.Log("Health", EnemyHealth, "EnemyStats"); // Can use same keys in different categories
Lebug.Log("Armor", EnemyArmor, "EnemyStats"); // Can use same keys in different categories
Lebug.Log("SongIndex", SongIndex); // Default category
if (RocketAmmo == 0)
{
Lebug.Del("Rocket Ammo"); // Delete a value.
}
if (EnemyHealth == 0)
{
Lebug.DelCategory("EnemyStats"); // Delete a category
}
}
// TODO: Should return immediately if socket is not open but easier to debug and calc speed, change in production
public void SocketSend(byte cmd, byte[] values)
{
int dataSize = values.Length + 5;
byte[] data = new byte[dataSize];
data[0] = 0x06;
data[1] = 0x85;
data[2] = deviceID;
data[3] = cmd;
byte cs = (byte)values.Length;
cs ^= deviceID;
cs ^= cmd;
for (int i = 0; i < values.Length; i++)
{
data[i + 4] = values[i];
cs ^= values[i];
}
data[data.Length - 1] = cs;
if (isOpen && sendActive)
{
socket.Send(data);
}
calcBytesPerSec(data.Length);
Lebug.Log("Last Command", System.BitConverter.ToString(data), "Socket"); // Debug
}
void ShowInfo()
{
for (int i = 0; i < JointEntities.Length - 1; i++)
{
JointEntity joint = JointEntities[i];
int jointAngle = Mathf.RoundToInt(WrapAngle(getActiveAngle(joint)));
Lebug.Log(joint.Joint.name, jointAngle + ":" + joint.servoAngle, "IKServo");
}
}
//////////////
// DEBUG
//////////////
void calcBytesPerSec(int dataLen)
{
dataSizePerSec += dataLen;
if (Time.time > oldTime)
{
// ~1 sec has passed
dataSizePerSec = 0;
oldTime = Time.time;
}
Lebug.Log("Bytes per sec", dataSizePerSec, "Socket"); // Debug
}
public void updateJointAngles(Transform newJoint)
{
Vector3 euler = Helpers.Math.normalizeEuler(newJoint.localRotation.eulerAngles);
Vector3 newEuler = Vector3.zero;
int activeAngle = (int)activeJointAngle;
if (isEnabled)
{
newEuler[activeAngle] = Mathf.Clamp(euler[activeAngle], minAngle, maxAngle);
}
transform.localEulerAngles = newEuler;
Lebug.Log(name, newEuler[activeAngle], "IKJoint");
}
void ShowStats()
{
Lebug.Log("Message Rate/s", currentPublishRate, "MQTT");
}
void ShowInfo()
{
Lebug.Log(joint.name, servoAngle + ":" + servoPWM, "IKServo");
}
void Solve()
{
Transform endEffector = joints[joints.Length - 1].transform;
Vector3 rootPos, curEnd;
Vector3 targetVector = Vector3.zero;
Vector3 currentVector = Vector3.zero;
Vector3 crossResult = Vector3.zero;
float cosAngle, turnAngle;
// START AT THE LAST LINK IN THE CHAIN
int link = joints.Length - 1;
int tries = 0;
// SEE IF I AM ALREADY CLOSE ENOUGH
do
{
if (link < 0)
{
link = joints.Length - 1;
}
Transform linkTransform = joints[link].transform;
rootPos = linkTransform.position;
curEnd = endEffector.position;
// CREATE THE VECTOR TO THE CURRENT EFFECTOR POS
currentVector = curEnd - rootPos;
// CREATE THE DESIRED EFFECTOR POSITION VECTOR
targetVector = Target.position - rootPos;
// NORMALIZE THE VECTORS
currentVector.Normalize();
targetVector.Normalize();
// THE DOT PRODUCT GIVES ME THE COSINE OF THE DESIRED ANGLE
cosAngle = Vector3.Dot(currentVector, targetVector);
// IF THE DOT PRODUCT RETURNS 1.0, I DON'T NEED TO ROTATE AS IT IS 0 DEGREES
if (cosAngle < 0.99999f)
{
// USE THE CROSS PRODUCT TO CHECK WHICH WAY TO ROTATE
crossResult = Vector3.Cross(currentVector, targetVector);
crossResult.Normalize();
turnAngle = Mathf.Acos(cosAngle);
// APPLY DAMPING
if (IsDamping)
{
if (turnAngle > DampingMax)
{
turnAngle = DampingMax;
}
}
turnAngle = turnAngle * Mathf.Rad2Deg;
linkTransform.rotation = Quaternion.AngleAxis(turnAngle, crossResult) * linkTransform.rotation;
joints[link].updateJointAngles(linkTransform);
}
link--;
Lebug.Log("IK Solving", counts, "IKSolver");
counts++;
} while (tries++ < MAX_IK_TRIES && (curEnd - Target.position).sqrMagnitude > IK_POS_THRESH);
}
