public static SBSVector3 RandomDirection(SBSVector3 direction, float angle)
{
SBSVector3 d = direction.normalized;
SBSVector3 t = SBSVector3.Cross(d + (SBSVector3)UnityEngine.Random.onUnitSphere, d).normalized;
return(SBSQuaternion.AngleAxis(UnityEngine.Random.value * angle, t) * d);
}
public void IncrementBy(SBSQuaternion q0)
{
x += q0.x;
y += q0.y;
z += q0.z;
w += q0.w;
}
public static SBSMatrix4x4 TRS(SBSVector3 t, SBSQuaternion q, SBSVector3 s)
{
SBSMatrix4x4 r = new SBSMatrix4x4();
float xy2 = 2.0f * q.x * q.y, xz2 = 2.0f * q.x * q.z, xw2 = 2.0f * q.x * q.w,
yz2 = 2.0f * q.y * q.z, yw2 = 2.0f * q.y * q.w, zw2 = 2.0f * q.z * q.w,
xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z, ww = q.w * q.w,
sx = s.x, sy = s.y, sz = s.z;
r.m00 = (xx - yy - zz + ww) * sx;
r.m01 = (xy2 + zw2) * sx;
r.m02 = (xz2 - yw2) * sx;
r.m03 = 0.0f;
r.m10 = (xy2 - zw2) * sy;
r.m11 = (-xx + yy - zz + ww) * sy;
r.m12 = (yz2 + xw2) * sy;
r.m13 = 0.0f;
r.m20 = (xz2 + yw2) * sz;
r.m21 = (yz2 - xw2) * sz;
r.m22 = (-xx - yy + zz + ww) * sz;
r.m23 = 0.0f;
r.m30 = t.x;
r.m31 = t.y;
r.m32 = t.z;
r.m33 = 1.0f;
return(r);
}
public void DecrementBy(SBSQuaternion q0)
{
x -= q0.x;
y -= q0.y;
z -= q0.z;
w -= q0.w;
}
public static SBSQuaternion Integrate(SBSQuaternion rotation, SBSVector3 angularVelocity, float dt)
{
SBSQuaternion spin = new SBSQuaternion(angularVelocity.x, angularVelocity.y, angularVelocity.z, 0.0f) * rotation;
spin.ScaleBy(0.5f * dt);
SBSQuaternion output = rotation + spin;
output.Normalize();
return(output);
}
public static SBSVector3 Rotate(SBSVector3 from, SBSVector3 to, float t)
{
SBSVector3 a = SBSVector3.Cross(from, to);
if (a.sqrMagnitude > 1.0e-3f)
{
return(SBSQuaternion.AngleAxis(SBSVector3.Angle(from, to) * t, a) * from);
}
else
{
return(to);
}
}
public static SBSQuaternion Slerp(SBSQuaternion q0, SBSQuaternion q1, float t)
{
float w1 = q0.w, x1 = q0.x, y1 = q0.y, z1 = q0.z,
w2 = q1.w, x2 = q1.x, y2 = q1.y, z2 = q1.z,
dot = w1 * w2 + x1 * x2 + y1 * y2 + z1 * z2;
if (dot < 0.0f)
{
dot = -dot;
w2 = -w2;
x2 = -x2;
y2 = -y2;
z2 = -z2;
}
if (dot < 0.95f)
{
float angle = SBSMath.Acos(dot),
s = 1.0f / SBSMath.Sin(angle),
s1 = SBSMath.Sin(angle * (1.0f - t)) * s,
s2 = SBSMath.Sin(angle * t) * s;
w1 = w1 * s1 + w2 * s2;
x1 = x1 * s1 + x2 * s2;
y1 = y1 * s1 + y2 * s2;
z1 = z1 * s1 + z2 * s2;
}
else
{
w1 += t * (w2 - w1);
x1 += t * (x2 - x1);
y1 += t * (y2 - y1);
z1 += t * (z2 - z1);
float oom = x1 * x1 + y1 * y1 + z1 * z1 + w1 * w1;
if (oom < SBSMath.SqrEpsilon)
{
return(new SBSQuaternion(0.0f, 0.0f, 0.0f, 1.0f));
}
oom = 1.0f / SBSMath.Sqrt(oom);
x1 *= oom;
y1 *= oom;
z1 *= oom;
w1 *= oom;
}
return(new SBSQuaternion(x1, y1, z1, w1));
}
public static void Slerp(SBSQuaternion q0, SBSQuaternion q1, float t, out SBSQuaternion o)
#endif
{
float w1 = q0.w, x1 = q0.x, y1 = q0.y, z1 = q0.z,
w2 = q1.w, x2 = q1.x, y2 = q1.y, z2 = q1.z,
dot = w1 * w2 + x1 * x2 + y1 * y2 + z1 * z2;
if (dot < 0.0f)
{
dot = -dot;
w2 = -w2;
x2 = -x2;
y2 = -y2;
z2 = -z2;
}
if (dot < 0.95f)
{
float angle = SBSMath.Acos(dot),
s = 1.0f / SBSMath.Sin(angle),
s1 = SBSMath.Sin(angle * (1.0f - t)) * s,
s2 = SBSMath.Sin(angle * t) * s;
o.w = w1 * s1 + w2 * s2;
o.x = x1 * s1 + x2 * s2;
o.y = y1 * s1 + y2 * s2;
o.z = z1 * s1 + z2 * s2;
}
else
{
o.w = w1 + t * (w2 - w1);
o.x = x1 + t * (x2 - x1);
o.y = y1 + t * (y2 - y1);
o.z = z1 + t * (z2 - z1);
o.Normalize();
}
}
public void TokenToWorld(float longitudinal, float trasversal, out SBSVector3 pos, out SBSVector3 tang)
#endif
{
switch (type)
{
case TokenType.Cross:
case TokenType.Rect:
#if UNITY_FLASH
matWorld.MultiplyPoint3x4(SBSVector3.right * (trasversal * width * 0.5f) + SBSVector3.forward * (longitudinal * lengthOrRadius), pos);
matWorld.MultiplyVector(SBSVector3.forward, tang);
#else
pos = matWorld.MultiplyPoint3x4(SBSVector3.right * (trasversal * width * 0.5f) + SBSVector3.forward * (longitudinal * lengthOrRadius));
tang = matWorld.MultiplyVector(SBSVector3.forward);
#endif
break;
case TokenType.Curve:
SBSVector3 center = (-curveDir * lengthOrRadius) * SBSVector3.right,
offset = ((trasversal * width * 0.5f) + (curveDir * lengthOrRadius)) * SBSVector3.right;
SBSMatrix4x4 matTr = SBSMatrix4x4.TRS(center, SBSQuaternion.identity, SBSVector3.one),
matRot = SBSMatrix4x4.TRS(SBSVector3.zero, SBSQuaternion.AngleAxis(-arcAngle * curveDir * longitudinal, SBSVector3.up), SBSVector3.one);
#if UNITY_FLASH
(matWorld * matTr * matRot).MultiplyPoint3x4(offset, pos);
(matWorld * matRot).MultiplyVector(SBSVector3.forward, tang);
#else
pos = (matWorld * matTr * matRot).MultiplyPoint3x4(offset);
tang = (matWorld * matRot).MultiplyVector(SBSVector3.forward);
#endif
break;
default:
pos.x = pos.y = pos.z = 0.0f;
tang.x = tang.y = tang.z = 0.0f;
break;
}
}
public void SetTRS(SBSVector3 t, SBSQuaternion q, SBSVector3 s)
{
float xy2 = 2.0f * q.x * q.y, xz2 = 2.0f * q.x * q.z, xw2 = 2.0f * q.x * q.w,
yz2 = 2.0f * q.y * q.z, yw2 = 2.0f * q.y * q.w, zw2 = 2.0f * q.z * q.w,
xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z, ww = q.w * q.w,
sx = s.x, sy = s.y, sz = s.z;
m00 = (xx - yy - zz + ww) * sx;
m01 = (xy2 + zw2) * sx;
m02 = (xz2 - yw2) * sx;
m03 = 0.0f;
m10 = (xy2 - zw2) * sy;
m11 = (-xx + yy - zz + ww) * sy;
m12 = (yz2 + xw2) * sy;
m13 = 0.0f;
m20 = (xz2 + yw2) * sz;
m21 = (yz2 - xw2) * sz;
m22 = (-xx - yy + zz + ww) * sz;
m23 = 0.0f;
m30 = t.x;
m31 = t.y;
m32 = t.z;
m33 = 1.0f;
}
public void DecrementBy(SBSQuaternion q0)
{
x -= q0.x;
y -= q0.y;
z -= q0.z;
w -= q0.w;
}
public void IncrementBy(SBSQuaternion q0)
{
x += q0.x;
y += q0.y;
z += q0.z;
w += q0.w;
}
public static void Slerp(SBSQuaternion q0, SBSQuaternion q1, float t, out SBSQuaternion o)
#endif
{
float w1 = q0.w, x1 = q0.x, y1 = q0.y, z1 = q0.z,
w2 = q1.w, x2 = q1.x, y2 = q1.y, z2 = q1.z,
dot = w1 * w2 + x1 * x2 + y1 * y2 + z1 * z2;
if (dot < 0.0f) {
dot = -dot;
w2 = -w2;
x2 = -x2;
y2 = -y2;
z2 = -z2;
}
if (dot < 0.95f) {
float angle = SBSMath.Acos(dot),
s = 1.0f / SBSMath.Sin(angle),
s1 = SBSMath.Sin(angle * (1.0f - t)) * s,
s2 = SBSMath.Sin(angle * t) * s;
o.w = w1 * s1 + w2 * s2;
o.x = x1 * s1 + x2 * s2;
o.y = y1 * s1 + y2 * s2;
o.z = z1 * s1 + z2 * s2;
} else {
o.w = w1 + t * (w2 - w1);
o.x = x1 + t * (x2 - x1);
o.y = y1 + t * (y2 - y1);
o.z = z1 + t * (z2 - z1);
o.Normalize();
}
}
public static void Slerp(SBSQuaternion q0, SBSQuaternion q1, float t, SBSQuaternion o)
public static SBSQuaternion Slerp(SBSQuaternion q0, SBSQuaternion q1, float t)
{
float w1 = q0.w, x1 = q0.x, y1 = q0.y, z1 = q0.z,
w2 = q1.w, x2 = q1.x, y2 = q1.y, z2 = q1.z,
dot = w1 * w2 + x1 * x2 + y1 * y2 + z1 * z2;
if (dot < 0.0f) {
dot = -dot;
w2 = -w2;
x2 = -x2;
y2 = -y2;
z2 = -z2;
}
if (dot < 0.95f) {
float angle = SBSMath.Acos(dot),
s = 1.0f / SBSMath.Sin(angle),
s1 = SBSMath.Sin(angle * (1.0f - t)) * s,
s2 = SBSMath.Sin(angle * t) * s;
w1 = w1 * s1 + w2 * s2;
x1 = x1 * s1 + x2 * s2;
y1 = y1 * s1 + y2 * s2;
z1 = z1 * s1 + z2 * s2;
} else {
w1 += t * (w2 - w1);
x1 += t * (x2 - x1);
y1 += t * (y2 - y1);
z1 += t * (z2 - z1);
float oom = x1 * x1 + y1 * y1 + z1 * z1 + w1 * w1;
if (oom < SBSMath.SqrEpsilon)
return new SBSQuaternion(0.0f, 0.0f, 0.0f, 1.0f);
oom = 1.0f / SBSMath.Sqrt(oom);
x1 *= oom;
y1 *= oom;
z1 *= oom;
w1 *= oom;
}
return new SBSQuaternion(x1, y1, z1, w1);
}
public void SetTRS(SBSVector3 t, SBSAngleAxis r, SBSVector3 s)
{
this.SetTRS(t, SBSQuaternion.AngleAxis(r.angle, r.axis), s);
}
public static SBSMatrix4x4 TRS(SBSVector3 t, SBSAngleAxis r, SBSVector3 s)
{
return(TRS(t, SBSQuaternion.AngleAxis(r.angle, r.axis), s));
}
public static SBSMatrix4x4 TRS(SBSVector3 t, SBSQuaternion q, SBSVector3 s)
{
SBSMatrix4x4 r = new SBSMatrix4x4();
float xy2 = 2.0f * q.x * q.y, xz2 = 2.0f * q.x * q.z, xw2 = 2.0f * q.x * q.w,
yz2 = 2.0f * q.y * q.z, yw2 = 2.0f * q.y * q.w, zw2 = 2.0f * q.z * q.w,
xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z, ww = q.w * q.w,
sx = s.x, sy = s.y, sz = s.z;
r.m00 = (xx - yy - zz + ww) * sx;
r.m01 = (xy2 + zw2) * sx;
r.m02 = (xz2 - yw2) * sx;
r.m03 = 0.0f;
r.m10 = (xy2 - zw2) * sy;
r.m11 = (-xx + yy - zz + ww) * sy;
r.m12 = (yz2 + xw2) * sy;
r.m13 = 0.0f;
r.m20 = (xz2 + yw2) * sz;
r.m21 = (yz2 - xw2) * sz;
r.m22 = (-xx - yy + zz + ww) * sz;
r.m23 = 0.0f;
r.m30 = t.x;
r.m31 = t.y;
r.m32 = t.z;
r.m33 = 1.0f;
return r;
}
public void SetTRS(SBSVector3 t, SBSQuaternion q, SBSVector3 s)
{
float xy2 = 2.0f * q.x * q.y, xz2 = 2.0f * q.x * q.z, xw2 = 2.0f * q.x * q.w,
yz2 = 2.0f * q.y * q.z, yw2 = 2.0f * q.y * q.w, zw2 = 2.0f * q.z * q.w,
xx = q.x * q.x, yy = q.y * q.y, zz = q.z * q.z, ww = q.w * q.w,
sx = s.x, sy = s.y, sz = s.z;
m00 = (xx - yy - zz + ww) * sx;
m01 = (xy2 + zw2) * sx;
m02 = (xz2 - yw2) * sx;
m03 = 0.0f;
m10 = (xy2 - zw2) * sy;
m11 = (-xx + yy - zz + ww) * sy;
m12 = (yz2 + xw2) * sy;
m13 = 0.0f;
m20 = (xz2 + yw2) * sz;
m21 = (yz2 - xw2) * sz;
m22 = (-xx - yy + zz + ww) * sz;
m23 = 0.0f;
m30 = t.x;
m31 = t.y;
m32 = t.z;
m33 = 1.0f;
}
public void CreateBoxColliders(int numSegments, float trasversalOffset, float wallsHeight, float depth)
{
if (numSegments < 1)
{
return;
}
BoxCollider[] colliders = gameObject.GetComponentsInChildren <BoxCollider>();
foreach (BoxCollider coll in colliders)
{
GameObject.DestroyImmediate(coll.gameObject);
}
colliders = null;
float l = 0.0f,
s = 1.0f / (float)numSegments;
for (int i = 0; i < numSegments; ++i)
{
#if UNITY_FLASH
SBSVector3 p0 = new SBSVector3(), p1 = new SBSVector3(), q0 = new SBSVector3(), q1 = new SBSVector3(), t = new SBSVector3();
this.TokenToWorld(l, -1.0f - trasversalOffset, p0, t);
this.TokenToWorld(l + s, -1.0f - trasversalOffset, p1, t);
this.TokenToWorld(l, 1.0f + trasversalOffset, q0, t);
this.TokenToWorld(l + s, 1.0f + trasversalOffset, q1, t);
#else
SBSVector3 p0, p1, q0, q1, t;
this.TokenToWorld(l, -1.0f - trasversalOffset, out p0, out t);
this.TokenToWorld(l + s, -1.0f - trasversalOffset, out p1, out t);
this.TokenToWorld(l, 1.0f + trasversalOffset, out q0, out t);
this.TokenToWorld(l + s, 1.0f + trasversalOffset, out q1, out t);
#endif
SBSVector3 tanL = p1 - p0,
tanR = q1 - q0,
biTan = q0 - p0;
float sizeX = biTan.Normalize(),
sizeZL = tanL.Normalize() + 0.2f,
sizeZR = tanR.Normalize() + 0.2f;
SBSQuaternion rotL = SBSQuaternion.AngleAxis(SBSVector3.Angle(SBSVector3.forward, tanL), SBSVector3.Cross(SBSVector3.forward, tanL)),
rotR = SBSQuaternion.AngleAxis(SBSVector3.Angle(SBSVector3.forward, tanR), SBSVector3.Cross(SBSVector3.forward, tanR));
SBSVector3 groundCenter = (p0 + p1 + q0 + q1) * 0.25f - SBSVector3.up * depth * 0.5f,
wallLeftCenter = (p0 + p1 + SBSVector3.up * wallsHeight - biTan * depth) * 0.5f,
wallRightCenter = (q0 + q1 + SBSVector3.up * wallsHeight + biTan * depth) * 0.5f,
groundSize = new SBSVector3(sizeX, depth, SBSMath.Max(sizeZL, sizeZR)),
wallLeftSize = new SBSVector3(depth, wallsHeight, sizeZL),
wallRightSize = new SBSVector3(depth, wallsHeight, sizeZR);
GameObject ground = new GameObject("Ground", typeof(BoxCollider));
ground.transform.parent = transform;
ground.transform.rotation = rotL;
ground.transform.position = groundCenter;
ground.GetComponent <BoxCollider>().size = groundSize;
GameObject wallLeft = new GameObject("WallLeft", typeof(BoxCollider)),
wallRight = new GameObject("WallRight", typeof(BoxCollider));
wallLeft.transform.parent = wallRight.transform.parent = transform;
wallLeft.transform.rotation = rotL;
wallLeft.GetComponent <BoxCollider>().size = wallLeftSize;
wallLeft.transform.position = wallLeftCenter;
wallRight.transform.rotation = rotR;
wallRight.GetComponent <BoxCollider>().size = wallRightSize;
wallRight.transform.position = wallRightCenter;
l += s;
}
}
public static void Slerp(SBSQuaternion q0, SBSQuaternion q1, float t, SBSQuaternion o)
