/// <summary>
/// Creates a matrix for rotating points around the Z-axis.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Z-axis.</param>
/// <returns>The rotation matrix.</returns>
public static TSMatrix4x4 RotateZ(FP radians)
{
TSMatrix4x4 result;
FP c = TSMath.Cos(radians);
FP s = TSMath.Sin(radians);
// [ c s 0 0 ]
// [ -s c 0 0 ]
// [ 0 0 1 0 ]
// [ 0 0 0 1 ]
result.M11 = c;
result.M12 = s;
result.M13 = FP.Zero;
result.M14 = FP.Zero;
result.M21 = -s;
result.M22 = c;
result.M23 = FP.Zero;
result.M24 = FP.Zero;
result.M31 = FP.Zero;
result.M32 = FP.Zero;
result.M33 = FP.One;
result.M34 = FP.Zero;
result.M41 = FP.Zero;
result.M42 = FP.Zero;
result.M43 = FP.Zero;
result.M44 = FP.One;
return(result);
}
/// <summary>
/// Creates a matrix for rotating points around the Y-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Y-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static TSMatrix4x4 RotateY(FP radians, TSVector centerPoint)
{
TSMatrix4x4 result;
FP c = TSMath.Cos(radians);
FP s = TSMath.Sin(radians);
FP x = centerPoint.x * (FP.One - c) - centerPoint.z * s;
FP z = centerPoint.x * (FP.One - c) + centerPoint.x * s;
// [ c 0 -s 0 ]
// [ 0 1 0 0 ]
// [ s 0 c 0 ]
// [ x 0 z 1 ]
result.M11 = c;
result.M12 = FP.Zero;
result.M13 = -s;
result.M14 = FP.Zero;
result.M21 = FP.Zero;
result.M22 = FP.One;
result.M23 = FP.Zero;
result.M24 = FP.Zero;
result.M31 = s;
result.M32 = FP.Zero;
result.M33 = c;
result.M34 = FP.Zero;
result.M41 = x;
result.M42 = FP.Zero;
result.M43 = z;
result.M44 = FP.One;
return(result);
}
/// <summary>
/// Creates a matrix for rotating points around the X-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the X-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static TSMatrix4x4 RotateX(FP radians, TSVector centerPoint)
{
TSMatrix4x4 result;
FP c = TSMath.Cos(radians);
FP s = TSMath.Sin(radians);
FP y = centerPoint.y * (FP.One - c) + centerPoint.z * s;
FP z = centerPoint.z * (FP.One - c) - centerPoint.y * s;
// [ 1 0 0 0 ]
// [ 0 c s 0 ]
// [ 0 -s c 0 ]
// [ 0 y z 1 ]
result.M11 = FP.One;
result.M12 = FP.Zero;
result.M13 = FP.Zero;
result.M14 = FP.Zero;
result.M21 = FP.Zero;
result.M22 = c;
result.M23 = s;
result.M24 = FP.Zero;
result.M31 = FP.Zero;
result.M32 = -s;
result.M33 = c;
result.M34 = FP.Zero;
result.M41 = FP.Zero;
result.M42 = y;
result.M43 = z;
result.M44 = FP.One;
return(result);
}
/// <summary>
/// Creates a matrix for rotating points around the Z-axis, from a center point.
/// </summary>
/// <param name="radians">The amount, in radians, by which to rotate around the Z-axis.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The rotation matrix.</returns>
public static TSMatrix4x4 RotateZ(FP radians, TSVector centerPoint)
{
TSMatrix4x4 result;
FP c = TSMath.Cos(radians);
FP s = TSMath.Sin(radians);
FP x = centerPoint.x * (1 - c) + centerPoint.y * s;
FP y = centerPoint.y * (1 - c) - centerPoint.x * s;
// [ c s 0 0 ]
// [ -s c 0 0 ]
// [ 0 0 1 0 ]
// [ x y 0 1 ]
result.M11 = c;
result.M12 = s;
result.M13 = FP.Zero;
result.M14 = FP.Zero;
result.M21 = -s;
result.M22 = c;
result.M23 = FP.Zero;
result.M24 = FP.Zero;
result.M31 = FP.Zero;
result.M32 = FP.Zero;
result.M33 = FP.One;
result.M34 = FP.Zero;
result.M41 = FP.Zero;
result.M42 = FP.Zero;
result.M43 = FP.Zero;
result.M44 = FP.One;
return(result);
}
/// <summary>
/// Creates a matrix which rotates around the given axis by the given angle.
/// </summary>
/// <param name="axis">The axis.</param>
/// <param name="angle">The angle.</param>
/// <param name="result">The resulting rotation matrix</param>
public static void AxisAngle(ref TSVector axis, FP angle, out TSMatrix4x4 result)
{
// a: angle
// x, y, z: unit vector for axis.
//
// Rotation matrix M can compute by using below equation.
//
// T T
// M = uu + (cos a)( I-uu ) + (sin a)S
//
// Where:
//
// u = ( x, y, z )
//
// [ 0 -z y ]
// S = [ z 0 -x ]
// [ -y x 0 ]
//
// [ 1 0 0 ]
// I = [ 0 1 0 ]
// [ 0 0 1 ]
//
//
// [ xx+cosa*(1-xx) yx-cosa*yx-sina*z zx-cosa*xz+sina*y ]
// M = [ xy-cosa*yx+sina*z yy+cosa(1-yy) yz-cosa*yz-sina*x ]
// [ zx-cosa*zx-sina*y zy-cosa*zy+sina*x zz+cosa*(1-zz) ]
//
FP x = axis.x, y = axis.y, z = axis.z;
FP sa = TSMath.Sin(angle), ca = TSMath.Cos(angle);
FP xx = x * x, yy = y * y, zz = z * z;
FP xy = x * y, xz = x * z, yz = y * z;
result.M11 = xx + ca * (FP.One - xx);
result.M12 = xy - ca * xy + sa * z;
result.M13 = xz - ca * xz - sa * y;
result.M14 = FP.Zero;
result.M21 = xy - ca * xy - sa * z;
result.M22 = yy + ca * (FP.One - yy);
result.M23 = yz - ca * yz + sa * x;
result.M24 = FP.Zero;
result.M31 = xz - ca * xz + sa * y;
result.M32 = yz - ca * yz - sa * x;
result.M33 = zz + ca * (FP.One - zz);
result.M34 = FP.Zero;
result.M41 = FP.Zero;
result.M42 = FP.Zero;
result.M43 = FP.Zero;
result.M44 = FP.One;
}
static void DrawVO(TSVector2 circleCenter, FP radius, TSVector2 origin)
{
FP alpha = TSMath.Atan2((origin - circleCenter).y, (origin - circleCenter).x);
FP gamma = radius / (origin - circleCenter).magnitude;
FP delta = gamma <= FP.One ? TSMath.Abs(TSMath.Acos(gamma)) : 0;
// Draw.Debug.CircleXZ(FromXZ(circleCenter), radius, Color.black, alpha - delta, alpha + delta);
TSVector2 p1 = new TSVector2(TSMath.Cos(alpha - delta), TSMath.Sin(alpha - delta)) * radius;
TSVector2 p2 = new TSVector2(TSMath.Cos(alpha + delta), TSMath.Sin(alpha + delta)) * radius;
TSVector2 p1t = -new TSVector2(-p1.y, p1.x);
TSVector2 p2t = new TSVector2(-p2.y, p2.x);
p1 += circleCenter;
p2 += circleCenter;
// Debug.DrawRay(FromXZ(p1), FromXZ(p1t).normalized * 100, Color.black);
// Debug.DrawRay(FromXZ(p2), FromXZ(p2t).normalized * 100, Color.black);
}
/** Creates a VO for avoiding another agent.
* \param center The position of the other agent relative to this agent.
* \param offset Offset of the velocity obstacle. For example to account for the agents' relative velocities.
* \param radius Combined radius of the two agents (radius1 + radius2).
* \param inverseDt 1 divided by the local avoidance time horizon (e.g avoid agents that we will hit within the next 2 seconds).
* \param inverseDeltaTime 1 divided by the time step length.
*/
public VO(TSVector2 center, TSVector2 offset, FP radius, FP inverseDt, FP inverseDeltaTime)
{
// Adjusted so that a parameter weightFactor of 1 will be the default ("natural") weight factor
this.weightFactor = 1;
weightBonus = 0;
//this.radius = radius;
TSVector2 globalCenter;
circleCenter = center * inverseDt + offset;
FP tmp = Sqr(center.LengthSquared() / (radius * radius));//exp(-tmp),
// tmp = 1 +tmp+tmp*tmp/2+tmp*tmp*tmp/6; //simple use Taylor's Formula
this.weightFactor = 4 * System.Math.Exp(-tmp.AsFloat()) + 1;// 4 /tmp + 1;//exp()
// Collision?
if (center.magnitude < radius)
{
colliding = true;
// 0.001 is there to make sure lin1.magnitude is not so small that the normalization
// below will return TSVector2.zero as that will make the VO invalid and it will be ignored.
line1 = center.normalized * (center.magnitude - radius - FP.One / 1000) * 3 / 10 * inverseDeltaTime;
dir1 = new TSVector2(line1.y, -line1.x).normalized;
line1 += offset;
cutoffDir = TSVector2.zero;
cutoffLine = TSVector2.zero;
dir2 = TSVector2.zero;
line2 = TSVector2.zero;
this.radius = 0;
}
else
{
colliding = false;
center *= inverseDt;
radius *= inverseDt;
globalCenter = center + offset;
// 0.001 is there to make sure cutoffDistance is not so small that the normalization
// below will return TSVector2.zero as that will make the VO invalid and it will be ignored.
var cutoffDistance = center.magnitude - radius + FP.One / 1000;
cutoffLine = center.normalized * cutoffDistance;
cutoffDir = new TSVector2(-cutoffLine.y, cutoffLine.x).normalized;
cutoffLine += offset;
FP alpha = TSMath.Atan2(-center.y, -center.x);
FP delta = TSMath.Abs(TSMath.Acos(radius / center.magnitude));
this.radius = radius;
// Bounding Lines
// Point on circle
line1 = new TSVector2(TSMath.Cos(alpha + delta), TSMath.Sin(alpha + delta));
// Vector tangent to circle which is the correct line tangent
// Note that this vector is normalized
dir1 = new TSVector2(line1.y, -line1.x);
// Point on circle
line2 = new TSVector2(TSMath.Cos(alpha - delta), TSMath.Sin(alpha - delta));
// Vector tangent to circle which is the correct line tangent
// Note that this vector is normalized
dir2 = new TSVector2(line2.y, -line2.x);
line1 = line1 * radius + globalCenter;
line2 = line2 * radius + globalCenter;
}
segmentStart = TSVector2.zero;
segmentEnd = TSVector2.zero;
segment = false;
}
void Start()
{
Debug.Log(TSMath.Cos(90 * FP.Deg2Rad));
}
