public MyQuat(MyVec vec, float _w)
{
x = vec.x;
y = vec.y;
z = vec.z;
w = _w;
}
// Update is called once per frame
void Update()
{
//acceleration & forces
perd.vel = new MyVec(magnuss.x, -gravity, perd.vel.z);
velocity = perd.vel;
magnuss = ((MyVec.Cross(perd.angularV, perd.vel)) * magnussCoef * airDens * crossSectionalArea * radius) * 0.5f;
drag = (MyVec.Scale(perd.vel, perd.vel) * proportionalCoef * crossSectionalArea * airDens) * -0.5f;
acc = (magnuss + drag + new MyVec(0, -gravity * mass, 0)) / mass;
MagnusX = magnuss.x;
DragX = drag.x;
DragY = drag.y;
DragZ = drag.z;
perd.pos.x += (magnuss.x - drag.x) * Time.deltaTime;
perd.pos.y -= (gravity - drag.y) * (Time.deltaTime);
perd.pos.z += (perd.vel.z - drag.z) * Time.deltaTime;
positions = perd.pos;
//rotation
float angle = perd.angularV.magnitude * Time.deltaTime;
angularD = perd.angularV;
rot = new MyQuat(angularD.x * Mathf.Sin(angle / 2), angularD.y * Mathf.Sin(angle / 2), angularD.z * Mathf.Sin(angle / 2), Mathf.Cos(angle / 2));
final = rot * new MyQuat(transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w);
transform.rotation = new Quaternion(final.x, final.y, final.z, final.w);
transform.position = new Vector3(perd.pos.x, perd.pos.y, perd.pos.z);
}
// Update is called once per frame
void Update()
{
if (Input.GetKeyDown(KeyCode.R))
{
rClicked = !rClicked;
}
else if (Input.GetKeyDown(KeyCode.G))
{
gClicked = !gClicked;
}
else if (Input.GetKeyDown(KeyCode.B))
{
bClicked = !bClicked;
}
else if (Input.GetKeyDown(KeyCode.Escape))
{
paused = !paused;
}
else if (Input.GetKeyDown(KeyCode.Space))
{
vent = new MyVec(Random.Range(-5f, 5f), Random.Range(-5f, 5f), Random.Range(-5f, 5f));
text.text = "Wind Velocity: X(" + vent.x + ")," + "Y(" + vent.y + ");" + "Z(" + vent.z + ")";
}
if (paused)
{
Time.timeScale = 0;
}
else
{
Time.timeScale = 0.01f;
}
}
public MyVec normalize()
{
normalized = this;
normalized.x = this.x / length();
normalized.y = this.y / length();
normalized.z = this.z / length();
return(normalized);
}
public static MyVec operator *(float p, MyVec q)
{
MyVec m = new MyVec();
m.x = q.x * p;
m.y = q.y * p;
m.z = q.z * p;
return(m);
}
public static MyVec operator +(MyVec q, MyVec p)
{
MyVec m = new MyVec();
m.x = q.x + p.x;
m.y = q.y + p.y;
m.z = q.z + p.z;
return(m);
}
public MyVec(MyVec vec)
{
x = vec.x;
y = vec.y;
z = vec.z;
magnitude = Mathf.Sqrt(x * x + y * y + z * z);
sqrMagnitude = x * x + y * y + z * z;
//normalized = new MyVector3(x / magnitude, y / magnitude, z / magnitude);
}
public static MyVec operator -(MyVec q, MyVec p)
{
MyVec m = new MyVec();
m.x = q.x - p.x;
m.y = q.y - p.y;
m.z = q.z - p.z;
return(m);
}
public MyVec crossProduct(MyVec v, MyVec u)
{
MyVec c = new MyVec();
c.x = v.y * u.z - v.z * u.y;
c.y = v.z * u.x - v.x * u.z;
c.z = v.x * u.y - v.y * u.x;
return(c);
}
public float Distance(MyVec a, MyVec b)
{
MyVec separation = new MyVec();
separation.x = a.x - b.x;
separation.y = a.y - b.y;
separation.z = a.z - b.z;
float res = separation.length();
return(res);
}
// Update is called once per frame
void Update()
{
if (Input.GetKeyDown(KeyCode.Space))
{
for (int i = 0; i < bullets.Count; i++)
{
Destroy(bullets[i]);
}
}
v = Input.mousePosition;
v.z = ZPos;
if (Input.GetKey(KeyCode.W))
{
ZPos += 0.05f;
}
else if (Input.GetKey(KeyCode.S))
{
ZPos -= 0.05f;
}
v = Camera.main.ScreenToWorldPoint(v);
transform.position = v;
if (isdown)
{
count += Time.deltaTime;
}
if (Input.GetMouseButtonDown(0))
{
isdown = true;
firstPos = new MyVec(Input.mousePosition.x, Input.mousePosition.y, Input.mousePosition.z);
//print(firstPos.x);
}
else if (Input.GetMouseButtonUp(0))
{
isdown = false;
//count += Time.deltaTime;
lastPos = new MyVec(Input.mousePosition.x, Input.mousePosition.y, Input.mousePosition.z);
//print(lastPos.x);
MyVec finalPos = (lastPos - firstPos);
finalPos.Normalize();
float vel = finalPos.magnitude / count;
if (GameObject.Find("perdigo") == null)
{
GameObject nou = Instantiate(perdigo, init.position, init.rotation);
vel = -Input.GetAxis("Mouse X") * 10f;
nou.GetComponent <movementrotation>().angularY = vel;
bullets.Add(nou);
}
//print(vel);
}
//MyVec v =new MyVec( Input.mousePosition.x, Input.mousePosition.y, -9.15f);
}
public static MyVec Normalize(MyVec value)
{
float mag = Mathf.Sqrt((value.x * value.x) + (value.y * value.y) + (value.z * value.z));
//return new MyVector3(value.x / mag, value.y / mag, value.z / mag);
if (mag > 1E-05f)
{
return(new MyVec(value.x / mag, value.y / mag, value.z / mag));
}
else
{
return(zero);
}
}
public MyQuat fromAxisAngle(float angle, MyVec axis)
{
//angle *= Mathf.Rad2Deg;
Debug.Log("angle :" + angle);
MyQuat q = new MyQuat();
q.w = Mathf.Cos(angle / 2);
q.x = axis.x * Mathf.Sin(angle / 2);
q.y = axis.y * Mathf.Sin(angle / 2);
q.z = axis.z * Mathf.Sin(angle / 2);
q.normalize();
Debug.Log("q.x" + q.x);
Debug.Log("q.y" + q.y);
Debug.Log("q.z" + q.z);
Debug.Log("q.w" + q.w);
return(q);
}
// Use this for initialization
void Start()
{
Time.timeScale = 0.01f;
magnussCoef = 0.15f;
airDens = 1;
radius = 0.5f;
proportionalCoef = 0.15f;
crossSectionalArea = (Mathf.Pow(radius, 2)) * Mathf.PI;
keys = GameObject.Find("InputManager").GetComponent <KeyManager>();
perd.pos = new MyVec(transform.position.x, transform.position.y, transform.position.z);
perd.vel = new MyVec(0, 0, 20) + keys.vent;
perd.angularV = new MyVec(0, angularY, 0);
uolo = new Vector3(perd.angularV.x, perd.angularV.y, perd.angularV.z);
gravity = 9.8f * mass;
//perd.forces = drag + gravity + magnuss;
move = new MyVec(transform.position.x, transform.position.y, transform.position.z);
init = new MyQuat(transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w);
}
//Static Methods
public static MyQuat AngleAxis(float angle, ref MyVec axis)
{
if (axis.sqrMagnitude == 0.0f)
{
return(identity);
}
MyQuat result = new MyQuat(0, 0, 0, 1);
float radians = angle * Mathf.Deg2Rad;
radians *= 0.5f;
axis.Normalize();
axis = axis * Mathf.Sin(radians);
result.x = axis.x;
result.y = axis.y;
result.z = axis.z;
result.w = Mathf.Cos(radians);
return(Normalize(result));
}
void Update()
{
// Copy the joints positions to work with
for (int i = 0; i < joints.Length; i++)
{
copy[i] = new MyVec(joints[i].position.x, joints[i].position.y, joints[i].position.z); //Copy the joints
if (i < joints.Length - 1)
{
distances[i] = MyVec.Distance(joints[i + 1].position, joints[i].position); //Calculate the distances
}
}
done = (copy[copy.Length - 1] - new MyVec(target.position.x, target.position.y, target.position.z)).magnitude < treshold_condition;
if (!done)
{
float targetRootDist = MyVec.Distance(copy[0], new MyVec(target.position.x, target.position.y, target.position.z));
// Update joint positions
if (targetRootDist > distances.Sum())
{
// The target is unreachable
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (new MyVec(target.position.x, target.position.y, target.position.z) - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = copy[i] * (1 - lambda) + new MyVec(target.position.x, target.position.y, target.position.z) * lambda;
}
}
else
{
MyVec b = copy[0];
float difA = (copy[copy.Length - 1] - new MyVec(target.position.x, target.position.y, target.position.z)).magnitude;
// The target is reachable
while (difA > treshold_condition)
{
// STAGE 1: FORWARD REACHING
copy[copy.Length - 1] = new MyVec(target.position.x, target.position.y, target.position.z);
for (int i = copy.Length - 2; i > 0; i--)
{
float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i] = copy[i + 1] * (1 - lambda) + copy[i] * lambda;
}
// STAGE 2: BACKWARD REACHING
copy[0] = b;
for (int i = 0; i < copy.Length - 1; i++)
{
float r = (copy[i + 1] - copy[i]).magnitude;
float lambda = distances[i] / r;
copy[i + 1] = copy[i] * (1 - lambda) + copy[i + 1] * lambda;
}
difA = (copy[copy.Length - 1] - new MyVec(target.position.x, target.position.y, target.position.z)).magnitude;
}
}
// Update original joint rotations
for (int i = 0; i < joints.Length - 1; i++)
{
//TODO
MyVec A = new MyVec(joints[i + 1].position - joints[i].position);
MyVec B = copy[i + 1] - copy[i];
float cosa = MyVec.Dot(MyVec.Normalize(A), MyVec.Normalize(B));
float sina = MyVec.Cross(MyVec.Normalize(A), MyVec.Normalize(B)).magnitude;
float alpha = Mathf.Atan2(sina, cosa) * Mathf.Rad2Deg;
MyVec myAxis = MyVec.Normalize(MyVec.Cross(A, B));
//Vector3 axis = new Vector3(myAxis.x, myAxis.y, myAxis.z);
MyQuat myQuat = MyQuat.AngleAxis(alpha, ref myAxis);
Quaternion quat = new Quaternion(myQuat.x, myQuat.y, myQuat.z, myQuat.w);
joints[i].rotation = quat * joints[i].rotation;
//joints[i].rotation = Quaternion.AngleAxis(alpha, axis) * joints[i].rotation;
joints[i + 1].position = new Vector3(copy[i + 1].x, copy[i + 1].y, copy[i + 1].z);
if (i == 2)
{
// print(joints[i].rotation.z);
}
if ((joints[i].rotation.z > 0.5f || joints[i].rotation.z < -0.5f) && i > 0)
{
joints[i].rotation = joints[i - 1].rotation;
//joints[i].rotation = new Quaternion(joints[i].rotation.x, joints[i].rotation.y, , joints[i - 1].rotation.w);
}
}
}
}
public static MyVec Cross(MyVec lhs, MyVec rhs)
{
return(new MyVec(lhs.y * rhs.z - lhs.z * rhs.y, lhs.z * rhs.x - lhs.x * rhs.z, lhs.x * rhs.y - lhs.y * rhs.x));
}
public static float Distance(MyVec lhs, MyVec rhs)
{
return((lhs - rhs).magnitude);
}
public static float Dot(MyVec lhs, MyVec rhs)
{
return(lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z);
}
public static MyVec Scale(MyVec lhs, MyVec rhs)
{
return(new MyVec(lhs.x * rhs.x, lhs.y * rhs.y, lhs.z * rhs.z));
}
void Update()
{
vecTarget = new MyVec(target.position.x, target.position.y, target.position.z);
copy[0] = new MyVec(joints[0].transform.position.x, joints[0].transform.position.y, joints[0].transform.position.z);
// Copy the joints positions to work with
//TODO
for (int i = 0; i < joints.Length - 1; i++)
{
copy[i + 1] = new MyVec(joints[i + 1].transform.position.x, joints[i + 1].transform.position.y, joints[i + 1].transform.position.z);
distances[i] = (joints[i + 1].position - joints[i].position).magnitude;
}
done = (copy[copy.Length - 1] - vecTarget).length() < treshold_condition;
if (!done)
{
float targetRootDist = new MyVec().Distance(copy[0], vecTarget);
// Update joint positions
if (targetRootDist > distances.Sum())
{
// The target is unreachable
for (int i = 0; i < joints.Length - 1; i++)
{
float r = new MyVec().Distance(vecTarget, copy[i]);
float lambda = distances[i] / r;
copy[i + 1] = (1 - lambda) * copy[i] + (lambda * vecTarget);
}
}
else
{
// The target is reachable
MyVec b = new MyVec(copy[0].x, copy[0].y, copy[0].z);
float r = new MyVec().Distance(copy[copy.Length - 1], vecTarget);
while (r > treshold_condition)
{
// STAGE 1: FORWARD REACHING
//TODO
copy[copy.Length - 1] = vecTarget;
for (int i = copy.Length - 2; i > 0; i--)
{
float l = new MyVec().Distance(copy[i + 1], copy[i]);
float lambda = distances[i] / l;
copy[i] = (1 - lambda) * copy[i + 1] + (lambda * copy[i]);
}
// STAGE 2: BACKWARD REACHING
//TODO
copy[0] = b;
for (int i = 0; i < copy.Length - 1; i++)
{
float l = new MyVec().Distance(copy[i + 1], copy[i]);
float lambda = distances[i] / l;
copy[i + 1] = (1 - lambda) * copy[i] + (lambda * copy[i + 1]);
}
r = new MyVec().Distance(copy[copy.Length - 1], vecTarget);
}
}
// Update original joint rotations
for (int i = 0; i < joints.Length - 1; i++)
{
//TODO
MyVec temp1 = new MyVec(joints[i + 1].position.x, joints[i + 1].position.y, joints[i + 1].position.z);
MyVec temp2 = new MyVec(joints[i].position.x, joints[i].position.y, joints[i].position.z);
MyVec init = temp1 - temp2;
init.normalize();
MyVec now = copy[i + 1] - copy[i];
now.normalize();
float cos = (new MyVec().dotProduct(init.normalized, now.normalized));
float sin = new MyVec().crossProduct(init.normalized, now.normalized).length();
print("cos" + cos);
print("sin" + sin);
float angle = Mathf.Atan2(sin, cos);//* Mathf.Rad2Deg;
MyVec axis = new MyVec().crossProduct(init.normalized, now.normalized).normalize();
MyQuat tempRot = new MyQuat(joints[i].rotation.x, joints[i].rotation.y, joints[i].rotation.z, joints[i].rotation.w);
/*print(axis.x);
* print(axis.y);
* print(axis.z);*/
if (angle != 0)
{
MyQuat finalRot = new MyQuat().fromAxisAngle(angle, axis.normalized) * tempRot;
/*print(finalRot.x);
* print(finalRot.y);
* print(finalRot.z);
* print(finalRot.w);*/
joints[i].rotation = new Quaternion(finalRot.x, finalRot.y, finalRot.z, finalRot.w);
}
Vector3 finalPos = new Vector3(copy[i].x, copy[i].y, copy[i].z);
joints[i].position = finalPos;
}
}
}
public float dotProduct(MyVec v, MyVec u)
{
float a = v.x * u.x + v.y * u.y + v.z * u.z;
return(a);
}
// Use this for initialization
void Start()
{
vent = new MyVec(Random.Range(-5f, 5f), Random.Range(-5f, 5f), Random.Range(-5f, 5f));
text.text = "Wind Velocity: X(" + vent.x + ")," + "Y(" + vent.y + ");" + "Z(" + vent.z + ")";
}
//Static Methods
public static float Angle(MyVec from, MyVec to)
{
//return Mathf.Acos(Dot(from,to) / (from.magnitude * to.magnitude));
return(Mathf.Acos(Mathf.Clamp(Dot(from, to) / (from.magnitude * to.magnitude), -1f, 1f)) * 57.29578f);
}