public Transform GetHangPoint(string name, TSVector curPosition, TSVector curForward, out TSVector position, out TSVector forward)
{
position = curPosition;
forward = curForward;
Transform transform = null;
bool hasHangPoint = false;
if (m_cHangPointItem != null)
{
var hangPointData = m_cHangPointItem.GetHangPointData(name);
if (hangPointData != null)
{
hasHangPoint = true;
FP nAngle = TSVector.Angle(TSVector.forward, curForward);
if (curForward.x < 0)
{
nAngle = 360 - nAngle;
}
TSQuaternion sQuat = TSQuaternion.AngleAxis(nAngle, TSVector.up);
position = curPosition + sQuat * hangPointData.position;
forward = sQuat * hangPointData.forward;
forward.Normalize();
}
}
if (m_cHangPointView != null)
{
transform = m_cHangPointView.GetHangPoint(name);
hasHangPoint = true;
}
if (!hasHangPoint)
{
CLog.LogError("对象" + this.name + "找不到挂点名为:" + name + "的挂点信息");
}
return(transform);
}
public bool StartRotateBySpeed(TSVector startForward, TSVector targetForward, FP angleSpeed)
{
if (startForward.IsZero() || targetForward.IsZero())
{
return(false);
}
startForward.Normalize();
m_sCurForward = m_sStartForward = startForward;
targetForward.Normalize();
m_sTargetForward = targetForward;
if ((startForward - targetForward).IsZero())
{
return(false);
}
m_sSpeed = angleSpeed / 360;
m_sStartRotation = TSQuaternion.identity;
m_sTargetRotation = TSQuaternion.FromToRotation(m_sStartForward, m_sTargetForward);
m_nRotating = true;
m_sLerp = 0;
if (null != OnStartRotate)
{
OnStartRotate(m_sCurForward, m_sCurForward);
}
return(true);
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
result = direction;
result.Normalize();
TSVector.Multiply(result, radius, out result);
}
/// <summary>
/// Normalizes the given vector.
/// </summary>
/// <param name="value">The vector which should be normalized.</param>
/// <returns>A normalized vector.</returns>
#region public static JVector Normalize(JVector value)
public static TSVector Normalize(TSVector value)
{
TSVector result;
TSVector.Normalize(ref value, out result);
return(result);
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector expandVector;
TSVector.Normalize(direction, out expandVector);
TSVector.Multiply(expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
FP min = TSVector.Dot(points[0], direction);
FP dot = TSVector.Dot(points[1], direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = TSVector.Dot(points[2], direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
TSVector.Add(points[minIndex], expandVector, out result);
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector exp;
TSVector.Normalize(ref direction, out exp);
exp *= sphericalExpansion;
FP min = TSVector.Dot(ref vecs[0], ref direction);
int minIndex = 0;
FP dot = TSVector.Dot(ref vecs[1], ref direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = TSVector.Dot(ref vecs[2], ref direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
result = vecs[minIndex] + exp;
}
private bool OverlapTest(ref CapsuleShape capsule, ref TSVector p1, ref TSVector p2,
ref SphereShape sphere, ref TSVector sphereCenter,
ref TSVector pa, ref TSVector pb, ref TSVector normal, ref FP penetration)
{
SegmentShape cap = SegmentShape.Pool.GetNew();
cap.P1 = p1;
cap.P2 = p2;
TSVector v;
FP r2 = capsule.Radius + sphere.Radius;
r2 *= r2;
FP sb;
cap.ClosestPointTo(ref sphereCenter, out sb, out pb);
SegmentShape.Pool.GiveBack(cap);
TSVector.Subtract(ref sphereCenter, ref pb, out normal);
if (normal.sqrMagnitude - r2 >= TSMath.Epsilon)
{
return(false);
}
penetration = (capsule.radius + sphere.radius) - normal.magnitude;
normal.Normalize();
TSVector.Multiply(ref normal, -sphere.Radius, out v);
TSVector.Add(ref sphereCenter, ref v, out pa);
TSVector.Multiply(ref normal, capsule.Radius, out v);
TSVector.Add(ref pb, ref v, out pb);
TSVector.Negate(ref normal, out normal);
return(true);
}
public void SupportMapping(ref TSVector direction, out TSVector result)
{
FP min = TSVector.Dot(owner.points[indices.I0].position, direction);
FP dot = TSVector.Dot(owner.points[indices.I1].position, direction);
TSVector minVertex = owner.points[indices.I0].position;
if (dot > min)
{
min = dot;
minVertex = owner.points[indices.I1].position;
}
dot = TSVector.Dot(owner.points[indices.I2].position, direction);
if (dot > min)
{
min = dot;
minVertex = owner.points[indices.I2].position;
}
TSVector exp;
TSVector.Normalize(direction, out exp);
exp *= owner.triangleExpansion;
result = minVertex + exp;
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
TSVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
TSVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
TSVector p1, p2, dp;
TSVector.Add(ref body1.position, ref r1, out p1);
TSVector.Add(ref body2.position, ref r2, out p2);
TSVector.Subtract(ref p2, ref p1, out dp);
FP deltaLength = dp.magnitude - distance;
if (behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= FP.Zero)
{
skipConstraint = true;
}
else if (behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= FP.Zero)
{
skipConstraint = true;
}
else
{
skipConstraint = false;
TSVector n = p2 - p1;
if (n.sqrMagnitude != FP.Zero) n.Normalize();
jacobian[0] = -FP.One * n;
jacobian[1] = -FP.One * (r1 % n);
jacobian[2] = FP.One * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ TSVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ TSVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
TSVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
TSVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
TSVector p1, p2, dp;
TSVector.Add(ref body1.position, ref r1, out p1);
TSVector.Add(ref body2.position, ref r2, out p2);
TSVector.Subtract(ref p2, ref p1, out dp);
TSVector l = TSVector.Transform(lineNormal, body1.orientation);
l.Normalize();
TSVector t = (p1 - p2) % l;
if (t.sqrMagnitude != FP.Zero)
{
t.Normalize();
}
t = t % l;
jacobian[0] = t; // linearVel Body1
jacobian[1] = (r1 + p2 - p1) % t; // angularVel Body1
jacobian[2] = -FP.One * t; // linearVel Body2
jacobian[3] = -FP.One * r2 % t; // angularVel Body2
effectiveMass = body1.inverseMass + body2.inverseMass
+ TSVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ TSVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
if (effectiveMass != 0)
{
effectiveMass = FP.One / effectiveMass;
}
bias = -(l % (p2 - p1)).magnitude * biasFactor * (FP.One / timestep);
CBFrame.Utils.Logger.Debug("line122 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line128 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line134 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
/// <summary>
/// Initializes a contact.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="point1">The collision point in worldspace</param>
/// <param name="point2">The collision point in worldspace</param>
/// <param name="n">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public void Initialize(RigidBody body1, RigidBody body2, ref TSVector point1, ref TSVector point2, ref TSVector n,
FP penetration, bool newContact, ContactSettings settings)
{
this.body1 = body1; this.body2 = body2;
this.normal = n; normal.Normalize();
this.p1 = point1; this.p2 = point2;
this.newContact = newContact;
TSVector.Subtract(ref p1, ref body1.position, out relativePos1);
TSVector.Subtract(ref p2, ref body2.position, out relativePos2);
TSVector.Transform(ref relativePos1, ref body1.invOrientation, out realRelPos1);
TSVector.Transform(ref relativePos2, ref body2.invOrientation, out realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
body1IsMassPoint = body1.isParticle;
body2IsMassPoint = body2.isParticle;
// Material Properties
if (newContact)
{
treatBody1AsStatic = body1.isStatic;
treatBody2AsStatic = body2.isStatic;
CBFrame.Utils.Logger.Debug("line812 body2.isStatic:" + body2.isStatic + ",body1.isStatic:" + body1.isStatic);
accumulatedNormalImpulse = FP.Zero;
accumulatedTangentImpulse = FP.Zero;
lostSpeculativeBounce = FP.Zero;
switch (settings.MaterialCoefficientMixing)
{
case ContactSettings.MaterialCoefficientMixingType.TakeMaximum:
staticFriction = TSMath.Max(body1.staticFriction, body2.staticFriction);
dynamicFriction = TSMath.Max(body1.staticFriction, body2.staticFriction);
restitution = TSMath.Max(body1.restitution, body2.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
staticFriction = TSMath.Min(body1.staticFriction, body2.staticFriction);
dynamicFriction = TSMath.Min(body1.staticFriction, body2.staticFriction);
restitution = TSMath.Min(body1.restitution, body2.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.UseAverage:
staticFriction = (body1.staticFriction + body2.staticFriction) * FP.Half;
dynamicFriction = (body1.staticFriction + body2.staticFriction) * FP.Half;
restitution = (body1.restitution + body2.restitution) * FP.Half;
break;
}
}
this.settings = settings;
}
public void SetForward(TSVector forward)
{
if (forward == TSVector.zero)
{
return;
}
forward.Normalize();
m_sLastForward = m_sCurForward;
m_sCurForward = forward;
}
private void SetForward(TSVector forward)
{
TSVector preForward = m_sCurForward;
m_sCurForward = forward;
m_sCurForward.Normalize();
if (null != OnRotate)
{
OnRotate(preForward, m_sCurForward);
}
}
/// <summary>
/// Constraints a point on a body to be fixed on a line
/// which is fixed on another body.
/// </summary>
/// <param name="body1"></param>
/// <param name="body2"></param>
/// <param name="lineStartPointBody1"></param>
/// <param name="lineDirection"></param>
/// <param name="pointBody2"></param>
public PointOnLine(RigidBody body1, RigidBody body2,
TSVector lineStartPointBody1, TSVector pointBody2) : base(body1, body2)
{
TSVector.Subtract(ref lineStartPointBody1, ref body1.position, out localAnchor1);
TSVector.Subtract(ref pointBody2, ref body2.position, out localAnchor2);
TSVector.Transform(ref localAnchor1, ref body1.invOrientation, out localAnchor1);
TSVector.Transform(ref localAnchor2, ref body2.invOrientation, out localAnchor2);
lineNormal = TSVector.Normalize(lineStartPointBody1 - pointBody2);
}
private void Steer()
{
movement = destination - tsRigidBody.position;
if (movement.sqrMagnitude < FP.One)
{
return;
}
movement.Normalize();
movement *= TrueSyncManager.DeltaTime * speed / currentScale;
tsRigidBody.position += movement;
}
private void OnJoystickMove(Vector2 vector2)
{
if (PvpPlayerMgr.Instance.mainPlayer != null && PvpPlayerMgr.Instance.mainPlayer.unit != null)
{
if (vector2.magnitude > minDirLen)
{
var dir = new TSVector(FP.FromFloat(vector2.x), 0, FP.FromFloat(vector2.y));
dir.Normalize();
PvpPlayerMgr.Instance.mainPlayer.unit.ReqMoveForward(dir);
}
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
TSVector dp;
TSVector.Subtract(ref body2.position, ref body1.position, out dp);
FP deltaLength = dp.magnitude - distance;
if (behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= FP.Zero)
{
skipConstraint = true;
}
else if (behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= FP.Zero)
{
skipConstraint = true;
}
else
{
skipConstraint = false;
TSVector n = dp;
if (n.sqrMagnitude != FP.Zero)
{
n.Normalize();
}
jacobian[0] = -FP.One * n;
//jacobian[1] = -FP.One * (r1 % n);
jacobian[1] = FP.One * n;
//jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass;
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
CBFrame.Utils.Logger.Debug("line148 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
}
CBFrame.Utils.Logger.Debug("line153 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[1];
}
CBFrame.Utils.Logger.Debug("line158 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
TSVector dp;
TSVector.Subtract(body2.position, body1.position, out dp);
FP deltaLength = dp.magnitude - distance;
if (behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= FP.Zero)
{
skipConstraint = true;
}
else if (behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= FP.Zero)
{
skipConstraint = true;
}
else
{
skipConstraint = false;
TSVector n = dp;
if (n.sqrMagnitude != FP.Zero)
{
n.Normalize();
}
jacobian[0] = -FP.One * n;
//jacobian[1] = -FP.One * (r1 % n);
jacobian[1] = FP.One * n;
//jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass;
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
if (!body1.isStatic)
{
body1.ApplyImpulse(accumulatedImpulse * jacobian[0]);
}
if (!body2.isStatic)
{
body2.ApplyImpulse(accumulatedImpulse * jacobian[1]);
}
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
TSVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
TSVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
TSVector p1, p2, dp;
TSVector.Add(ref body1.position, ref r1, out p1);
TSVector.Add(ref body2.position, ref r2, out p2);
TSVector.Subtract(ref p2, ref p1, out dp);
FP deltaLength = dp.magnitude;
TSVector n = p2 - p1;
if (n.sqrMagnitude != FP.Zero)
{
n.Normalize();
}
jacobian[0] = -FP.One * n;
jacobian[1] = -FP.One * (r1 % n);
jacobian[2] = FP.One * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ TSVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ TSVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
CBFrame.Utils.Logger.Debug("line117 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line123 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line129 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
/// <summary>
///
/// </summary>
/// <param name="rayOrigin"></param>
/// <param name="rayDelta"></param>
/// <returns></returns>
public override int Prepare(ref TSVector rayOrigin, ref TSVector rayDelta)
{
potentialTriangles.Clear();
#region Expand Spherical
TSVector expDelta;
TSVector.Normalize(ref rayDelta, out expDelta);
expDelta = rayDelta + expDelta * sphericalExpansion;
#endregion
octree.GetTrianglesIntersectingRay(potentialTriangles, rayOrigin, expDelta);
return(potentialTriangles.Count);
}
/// <summary>
/// is the target infront of ship.
/// </summary>
/// <param name="targetpos">the target to check with</param>
/// <returns></returns>
bool FrontTest()
{
TSVector fwd = transformts.forward;
TSVector vec = localtarget - transformts.position;
vec.Normalize();
FP ang = TSMath.Acos(TSVector.Dot(fwd, vec)) * Mathf.Rad2Deg;
if (ang <= 45.0f)
{
return(true);
}
return(false);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
TSVector.Transform(localAnchor1, body1.orientation, out r1);
TSVector.Transform(localAnchor2, body2.orientation, out r2);
TSVector p1, p2, dp;
TSVector.Add(body1.position, r1, out p1);
TSVector.Add(body2.position, r2, out p2);
TSVector.Subtract(p2, p1, out dp);
FP deltaLength = dp.magnitude;
TSVector n = p2 - p1;
if (n.sqrMagnitude != FP.Zero)
{
n.Normalize();
}
jacobian[0] = -FP.One * n;
jacobian[1] = -FP.One * (r1 % n);
jacobian[2] = FP.One * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ TSVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ TSVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
if (!body1.isStatic)
{
body1.ApplyImpulse(accumulatedImpulse * jacobian[0]);
body1.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.ApplyImpulse(accumulatedImpulse * jacobian[2]);
body2.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
/// <summary>
///
/// </summary>
/// <param name="rayOrigin"></param>
/// <param name="rayDelta"></param>
/// <returns></returns>
public override int Prepare(ref TSVector rayOrigin, ref TSVector rayDelta)
{
TSBBox box = TSBBox.SmallBox;
#region RayEnd + Expand Spherical
TSVector rayEnd;
TSVector.Normalize(rayDelta, out rayEnd);
rayEnd = rayOrigin + rayDelta + rayEnd * sphericalExpansion;
#endregion
box.AddPoint(rayOrigin);
box.AddPoint(rayEnd);
return(this.Prepare(ref box));
}
public static void LookAt(TSVector forward, TSVector upwards, out TSMatrix result)
{
TSVector zaxis = forward; zaxis.Normalize();
TSVector xaxis = TSVector.Cross(upwards, zaxis); xaxis.Normalize();
TSVector yaxis = TSVector.Cross(zaxis, xaxis);
result.M11 = xaxis.x;
result.M21 = yaxis.x;
result.M31 = zaxis.x;
result.M12 = xaxis.y;
result.M22 = yaxis.y;
result.M32 = zaxis.y;
result.M13 = xaxis.z;
result.M23 = yaxis.z;
result.M33 = zaxis.z;
}
public static TSQuaternion AngleAxis(FP angle, TSVector axis)
{
axis = axis * FP.Deg2Rad;
axis.Normalize();
FP halfAngle = angle * FP.Deg2Rad * FP.Half;
TSQuaternion rotation;
FP sin = FP.Sin(halfAngle);
rotation.x = axis.x * sin;
rotation.y = axis.y * sin;
rotation.z = axis.z * sin;
rotation.w = FP.Cos(halfAngle);
return(rotation);
}
private bool DequeuePoint(TSVector curPosition, out TSVector forward)
{
forward = TSVector.forward;
if (m_queuePath.Count > 0)
{
while (m_queuePath.Count > 0)
{
m_sNextPosition = m_queuePath.Dequeue();
forward = m_sNextPosition - curPosition;
if (!forward.IsZero())
{
forward.Normalize();
return(true);
}
}
}
return(false);
}
public void StartMove(TSVector startPosition, List <TSVector> lstPosition)
{
TSVector forward = m_sCurForward;
if (lstPosition.Count > 0)
{
forward = lstPosition[0] - startPosition;
}
if (!forward.IsZero())
{
forward.Normalize();
}
SetForward(forward);
SetPosition(startPosition);
List <Vector3> lst = GameInTool.TSVectorToLstUnityVector3(lstPosition);
m_cLerpView.StartMove(transform.position, lst);
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector exp;
TSVector.Normalize(ref direction, out exp);
exp *= sphericalExpansion;
FP min = TSVector.Dot(ref vecs[0], ref direction);
int minIndex = 0;
for (int i = 1, length = vecs.Length; i < length; i++)
{
FP dot = TSVector.Dot(ref vecs[i], ref direction);
if (dot > min)
{
min = dot;
minIndex = i;
}
}
result = vecs[minIndex] + exp;
}
public LimitHingeJoint3D(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis, FP minLimit, FP maxLimit) : base(world, body1, body2, position, hingeAxis)
{
TSVector perpDir = TSVector.up;
if (TSVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = TSVector.right;
}
TSVector sideAxis = TSVector.Cross(hingeAxis, perpDir);
perpDir = TSVector.Cross(sideAxis, hingeAxis);
perpDir.Normalize();
FP len = 15;
TSVector hingeRelAnchorPos0 = perpDir * len;
FP angleToMiddle = FP.Half * (minLimit - maxLimit);
TSMatrix outMatrix;
TSMatrix.CreateFromAxisAngle(ref hingeAxis, -angleToMiddle * FP.Deg2Rad, out outMatrix);
TSVector hingeRelAnchorPos1 = TSVector.Transform(hingeRelAnchorPos0, outMatrix);
FP hingeHalfAngle = FP.Half * (minLimit + maxLimit);
FP allowedDistance = len * 2 * FP.Sin(hingeHalfAngle * FP.Half * FP.Deg2Rad);
TSVector hingePos = body1.TSPosition;
TSVector relPos0c = hingePos + hingeRelAnchorPos0;
TSVector relPos1c = hingePos + hingeRelAnchorPos1;
distance = new PointPointDistance((RigidBody)body1, (RigidBody)body2, relPos0c, relPos1c);
distance.Distance = allowedDistance;
distance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
StateTracker.AddTracking(distance);
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector expandVector;
TSVector.Normalize(ref direction, out expandVector);
TSVector.Multiply(ref expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
FP min = TSVector.Dot(ref points[0], ref direction);
for (int i = 1, length = points.Length; i < length; i++)
{
FP dot = TSVector.Dot(ref points[i], ref direction);
if (dot > min)
{
min = dot;
minIndex = i;
}
}
TSVector.Add(ref points[minIndex], ref expandVector, out result);
}