/// <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 FPVector direction, out FPVector result)
{
FPVector expandVector;
FPVector.Normalize(ref direction, out expandVector);
FPVector.Multiply(ref expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
FP min = FPVector.Dot(ref points[0], ref direction);
FP dot = FPVector.Dot(ref points[1], ref direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = FPVector.Dot(ref points[2], ref direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
FPVector.Add(ref points[minIndex], ref 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 FPVector direction, out FPVector result)
{
FPVector exp;
FPVector.Normalize(ref direction, out exp);
exp *= sphericalExpansion;
FP min = FPVector.Dot(ref vecs[0], ref direction);
int minIndex = 0;
FP dot = FPVector.Dot(ref vecs[1], ref direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = FPVector.Dot(ref vecs[2], ref direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
result = vecs[minIndex] + exp;
}
/// <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 FPMatrix4x4 RotateZ(FP radians, FPVector centerPoint)
{
FPMatrix4x4 result;
FP c = FPMath.Cos(radians);
FP s = FPMath.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);
}
public void SupportMapping(ref FPVector direction, out FPVector result)
{
FP min = FPVector.Dot(ref owner.points[indices.I0].position, ref direction);
FP dot = FPVector.Dot(ref owner.points[indices.I1].position, ref direction);
FPVector minVertex = owner.points[indices.I0].position;
if (dot > min)
{
min = dot;
minVertex = owner.points[indices.I1].position;
}
dot = FPVector.Dot(ref owner.points[indices.I2].position, ref direction);
if (dot > min)
{
min = dot;
minVertex = owner.points[indices.I2].position;
}
FPVector exp;
FPVector.Normalize(ref direction, out exp);
exp *= owner.triangleExpansion;
result = minVertex + exp;
}
public void Rotate(FPMatrix orientation, FPVector center)
{
for (int i = 0; i < points.Count; i++)
{
points[i].position = FPVector.Transform(points[i].position - center, orientation);
}
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
FP jv =
body1.linearVelocity * jacobian[0] +
body1.angularVelocity * jacobian[1] +
body2.linearVelocity * jacobian[2] +
body2.angularVelocity * jacobian[3];
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + bias + softnessScalar);
accumulatedImpulse += lambda;
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * lambda * jacobian[0];
body1.angularVelocity += FPVector.Transform(lambda * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * lambda * jacobian[2];
body2.angularVelocity += FPVector.Transform(lambda * jacobian[3], body2.invInertiaWorld);
}
}
public void Clone(Shape sh)
{
this.inertia = sh.inertia;
this.mass = sh.mass;
this.boundingBox = sh.boundingBox;
this.geomCen = sh.geomCen;
if (sh is BoxShape)
{
CloneBox((BoxShape)sh);
}
else if (sh is SphereShape)
{
CloneSphere((SphereShape)sh);
}
else if (sh is ConeShape)
{
CloneCone((ConeShape)sh);
}
else if (sh is CylinderShape)
{
CloneCylinder((CylinderShape)sh);
}
else if (sh is CapsuleShape)
{
CloneCapsule((CapsuleShape)sh);
}
}
/// <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 FPVector direction, out FPVector result)
{
FPVector.Transform(ref direction, ref shapes[currentShape].invOrientation, out result);
shapes[currentShape].Shape.SupportMapping(ref direction, out result);
FPVector.Transform(ref result, ref shapes[currentShape].orientation, out result);
FPVector.Add(ref result, ref shapes[currentShape].position, out result);
}
/// <summary>
/// Uses the supportMapping to calculate the bounding box. Should be overidden
/// to make this faster.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The resulting axis aligned bounding box.</param>
public virtual void GetBoundingBox(ref FPMatrix orientation, out FPBBox box)
{
// I don't think that this can be done faster.
// 6 is the minimum number of SupportMap calls.
FPVector vec = FPVector.zero;
vec.Set(orientation.M11, orientation.M21, orientation.M31);
SupportMapping(ref vec, out vec);
box.max.x = orientation.M11 * vec.x + orientation.M21 * vec.y + orientation.M31 * vec.z;
vec.Set(orientation.M12, orientation.M22, orientation.M32);
SupportMapping(ref vec, out vec);
box.max.y = orientation.M12 * vec.x + orientation.M22 * vec.y + orientation.M32 * vec.z;
vec.Set(orientation.M13, orientation.M23, orientation.M33);
SupportMapping(ref vec, out vec);
box.max.z = orientation.M13 * vec.x + orientation.M23 * vec.y + orientation.M33 * vec.z;
vec.Set(-orientation.M11, -orientation.M21, -orientation.M31);
SupportMapping(ref vec, out vec);
box.min.x = orientation.M11 * vec.x + orientation.M21 * vec.y + orientation.M31 * vec.z;
vec.Set(-orientation.M12, -orientation.M22, -orientation.M32);
SupportMapping(ref vec, out vec);
box.min.y = orientation.M12 * vec.x + orientation.M22 * vec.y + orientation.M32 * vec.z;
vec.Set(-orientation.M13, -orientation.M23, -orientation.M33);
SupportMapping(ref vec, out vec);
box.min.z = orientation.M13 * vec.x + orientation.M23 * vec.y + orientation.M33 * vec.z;
}
//return/calculate the closest vertex
public bool Closest(out FPVector v)
{
bool succes = UpdateClosestVectorAndPoints();
v = _cachedV;
return(succes);
}
public override void CalculateMassInertia()
{
base.inertia = FPMatrix.Zero;
base.mass = FP.Zero;
for (int i = 0; i < Shapes.Length; i++)
{
FPMatrix currentInertia = Shapes[i].InverseOrientation * Shapes[i].Shape.Inertia * Shapes[i].Orientation;
FPVector p = Shapes[i].Position * -FP.One;
FP m = Shapes[i].Shape.Mass;
currentInertia.M11 += m * (p.y * p.y + p.z * p.z);
currentInertia.M22 += m * (p.x * p.x + p.z * p.z);
currentInertia.M33 += m * (p.x * p.x + p.y * p.y);
currentInertia.M12 += -p.x * p.y * m;
currentInertia.M21 += -p.x * p.y * m;
currentInertia.M31 += -p.x * p.z * m;
currentInertia.M13 += -p.x * p.z * m;
currentInertia.M32 += -p.y * p.z * m;
currentInertia.M23 += -p.y * p.z * m;
base.inertia += currentInertia;
base.mass += m;
}
}
/// <summary>
/// Adds a contact to the arbiter (threadsafe). No more than four contacts
/// are stored in the contactList. When adding a new contact
/// to the arbiter the existing are checked and the best are kept.
/// </summary>
/// <param name="point1">Point on body1. In world space.</param>
/// <param name="point2">Point on body2. In world space.</param>
/// <param name="normal">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public Contact AddContact(FPVector point1, FPVector point2, FPVector normal, FP penetration,
ContactSettings contactSettings)
{
FPVector relPos1;
FPVector.Subtract(ref point1, ref body1.position, out relPos1);
int index;
lock (contactList)
{
if (this.contactList.Count == 4)
{
index = SortCachedPoints(ref relPos1, penetration);
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
index = GetCacheEntry(ref relPos1, contactSettings.breakThreshold);
if (index >= 0)
{
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
else
{
Contact contact = Contact.Pool.GetNew();
contact.Initialize(body1, body2, ref point1, ref point2, ref normal, penetration, true, contactSettings);
contactList.Add(contact);
return(contact);
}
}
}
public void Clone(Contact contact)
{
settings = contact.settings;
body1 = contact.body1;
body2 = contact.body2;
normal = contact.normal;
tangent = contact.tangent;
realRelPos1 = contact.realRelPos1;
realRelPos2 = contact.realRelPos2;
relativePos1 = contact.relativePos1;
relativePos2 = contact.relativePos2;
p1 = contact.p1;
p2 = contact.p2;
accumulatedNormalImpulse = contact.accumulatedNormalImpulse;
accumulatedTangentImpulse = contact.accumulatedTangentImpulse;
penetration = contact.penetration;
initialPen = contact.initialPen;
staticFriction = contact.staticFriction;
dynamicFriction = contact.dynamicFriction;
restitution = contact.restitution;
friction = contact.friction;
massNormal = contact.massNormal;
massTangent = contact.massTangent;
restitutionBias = contact.restitutionBias;
newContact = contact.newContact;
treatBody1AsStatic = contact.treatBody1AsStatic;
treatBody2AsStatic = contact.treatBody2AsStatic;
body1IsMassPoint = contact.body1IsMassPoint;
body2IsMassPoint = contact.body2IsMassPoint;
lostSpeculativeBounce = contact.lostSpeculativeBounce;
speculativeVelocity = contact.speculativeVelocity;
lastTimeStep = contact.lastTimeStep;
}
/// <summary>
/// Creates a JMatrix representing an orientation from a quaternion.
/// </summary>
/// <param name="quaternion">The quaternion the matrix should be created from.</param>
/// <returns>JMatrix representing an orientation.</returns>
#region public static JMatrix CreateFromQuaternion(JQuaternion quaternion)
public static FPMatrix CreateFromLookAt(FPVector position, FPVector target)
{
FPMatrix result;
LookAt(target - position, FPVector.up, out result);
return(result);
}
private void MoveStep()
{
if (!(path != null && path.Length > 0 && stepIndex < path.Length))
{
return;
}
nodePostion = GridMap.GetCenterPoint(path[stepIndex].x, path[stepIndex].y);
nodePostion = GetOnGroundPoint(nodePostion);
if (FPVector.Distance(FPTransform.position, nodePostion) < Speed * FrameSyncManager.DeltaTime)
{
FPTransform.position = nodePostion;
if (stepIndex < path.Length)
{
if (stepIndex == path.Length - 1)
{
velocity = FPVector.zero;
}
stepIndex++;
}
}
else
{
velocity = (nodePostion - FPTransform.position).normalized * Speed * FrameSyncManager.DeltaTime;
FPTransform.LookAt(new FPVector(nodePostion.x, FPTransform.position.y, nodePostion.z));
FPTransform.Translate(velocity, Space.World);
}
// Debug.LogWarningFormat("{0} FPTransform.position:{1},endNode:{2},stepIndex:{3},Length:{4},node:{5},position:{6}",
// name, GridMap.getIndex(FPTransform.position), endNode, stepIndex, path.Length, path[stepIndex],FPTransform.position);
}
/// <summary>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(FPVector force, FPVector pos)
{
FPVector.Add(ref this.force, ref force, out this.force);
FPVector.Subtract(ref pos, ref this.position, out pos);
FPVector.Cross(ref pos, ref force, out pos);
FPVector.Add(ref pos, ref this.torque, out this.torque);
}
private static SubKnockdownOptions UpgradeKnockdownOptions(SubKnockdownOptions knockDown)
{
knockDown._knockedOutTime = knockDown.knockedOutTime;
knockDown._standUpTime = knockDown.standUpTime;
knockDown._predefinedPushForce = FPVector.ToFPVector(knockDown.predefinedPushForce);
return(knockDown);
}
internal void SweptExpandBoundingBox(FP timestep)
{
sweptDirection = linearVelocity * timestep;
if (sweptDirection.x < FP.Zero)
{
boundingBox.min.x += sweptDirection.x;
}
else
{
boundingBox.max.x += sweptDirection.x;
}
if (sweptDirection.y < FP.Zero)
{
boundingBox.min.y += sweptDirection.y;
}
else
{
boundingBox.max.y += sweptDirection.y;
}
if (sweptDirection.z < FP.Zero)
{
boundingBox.min.z += sweptDirection.z;
}
else
{
boundingBox.max.z += sweptDirection.z;
}
}
/// <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 FPVector direction, out FPVector result)
{
result = direction;
result.Normalize();
FPVector.Multiply(ref result, radius, out result);
}
/// <summary>
/// Recalculates the axis aligned bounding box and the inertia
/// values in world space.
/// </summary>
public virtual void Update()
{
if (isParticle)
{
this.inertia = FPMatrix.Zero;
this.invInertia = this.invInertiaWorld = FPMatrix.Zero;
this.invOrientation = this.orientation = FPMatrix.Identity;
this.boundingBox = shape.boundingBox;
FPVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min);
FPVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max);
angularVelocity.MakeZero();
}
else
{
// Given: Orientation, Inertia
FPMatrix.Transpose(ref orientation, out invOrientation);
this.Shape.GetBoundingBox(ref orientation, out boundingBox);
FPVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min);
FPVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max);
if (!isStatic)
{
FPMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld);
FPMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld);
}
}
}
/// <summary>
/// 摇杆操作
/// </summary>
/// <param name="move"></param>
void OnJoystickMove(MovingJoystick move)
{
if (_avatarObj == null)
{
return;
}
if (move.joystickName != "New joystick")
{
return;
}
_positionX = move.joystickAxis.x; // 获取摇杆偏移摇杆中心的x坐标
_positionY = move.joystickAxis.y; // 获取摇杆偏移摇杆中心的y坐标
if (_bBeforeDragging)
{
_bBeforeDragging = false;
// _avatarObj.ChangeAvatarForward(ComMoveControllerObj.GetFPTransform().forward);
_fpChangeAvatarForward = ComMoveControllerObj.GetFPTransform().forward;
_fpDragFlag = true;
Debug.Log("_bBeforeDragging_bBeforeDragging_bBeforeDragging::" + _fpChangeAvatarForward.ToVector());
}
FrameSyncInput.SetFP((byte)E_InputId.E_MOVE_X, _positionX);
FrameSyncInput.SetFP((byte)E_InputId.E_MOVE_Y, _positionY);
}
public void Query(FPVector origin, FPVector direction, List <int> collisions)
{
Stack <int> stack = stackPool.GetNew();
stack.Push(_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
DynamicTreeNode <T> node = _nodes[nodeId];
if (node.AABB.RayIntersect(ref origin, ref direction))
{
if (node.IsLeaf())
{
collisions.Add(nodeId);
}
else
{
if (_nodes[node.Child1].AABB.RayIntersect(ref origin, ref direction))
{
stack.Push(node.Child1);
}
if (_nodes[node.Child2].AABB.RayIntersect(ref origin, ref direction))
{
stack.Push(node.Child2);
}
}
}
}
stackPool.GiveBack(stack);
}
public void SupportCenter(out FPVector center)
{
center = owner.points[indices.I0].position;
FPVector.Add(ref center, ref owner.points[indices.I1].position, out center);
FPVector.Add(ref center, ref owner.points[indices.I2].position, out center);
FPVector.Multiply(ref center, FP.One / (3 * FP.One), out center);
}
private void SupportMapping(RigidBody body, Shape workingShape, ref FPVector direction, out FPVector result)
{
FPVector.Transform(ref direction, ref body.invOrientation, out result);
workingShape.SupportMapping(ref result, out result);
FPVector.Transform(ref result, ref body.orientation, out result);
FPVector.Add(ref result, ref body.position, out result);
}
/// <summary>
/// Raycasts a single body. NOTE: For performance reasons terrain and trianglemeshshape aren't checked
/// against rays (rays are of infinite length). They are checked against segments
/// which start at rayOrigin and end in rayOrigin + rayDirection.
/// </summary>
#region public override bool Raycast(RigidBody body, JVector rayOrigin, JVector rayDirection, out JVector normal, out FP fraction)
public override bool Raycast(RigidBody body, FPVector rayOrigin, FPVector rayDirection, out FPVector normal, out FP fraction)
{
fraction = FP.MaxValue; normal = FPVector.zero;
if (!body.BoundingBox.RayIntersect(ref rayOrigin, ref rayDirection))
{
return(false);
}
if (body.Shape is Multishape)
{
Multishape ms = (body.Shape as Multishape).RequestWorkingClone();
FPVector tempNormal; FP tempFraction;
bool multiShapeCollides = false;
FPVector transformedOrigin; FPVector.Subtract(ref rayOrigin, ref body.position, out transformedOrigin);
FPVector.Transform(ref transformedOrigin, ref body.invOrientation, out transformedOrigin);
FPVector transformedDirection; FPVector.Transform(ref rayDirection, ref body.invOrientation, out transformedDirection);
int msLength = ms.Prepare(ref transformedOrigin, ref transformedDirection);
for (int i = 0; i < msLength; i++)
{
ms.SetCurrentShape(i);
if (GJKCollide.Raycast(ms, ref body.orientation, ref body.invOrientation, ref body.position,
ref rayOrigin, ref rayDirection, out tempFraction, out tempNormal))
{
if (tempFraction < fraction)
{
if (useTerrainNormal && ms is TerrainShape)
{
(ms as TerrainShape).CollisionNormal(out tempNormal);
FPVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
else if (useTriangleMeshNormal && ms is TriangleMeshShape)
{
(ms as TriangleMeshShape).CollisionNormal(out tempNormal);
FPVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
normal = tempNormal;
fraction = tempFraction;
multiShapeCollides = true;
}
}
}
ms.ReturnWorkingClone();
return(multiShapeCollides);
}
else
{
return(GJKCollide.Raycast(body.Shape, ref body.orientation, ref body.invOrientation, ref body.position,
ref rayOrigin, ref rayDirection, out fraction, out normal));
}
}
public Fix64 TestCollision(FPVector myRootPosition, FPVector opRootPosition, HitBox[] opHitBoxes)
{
Fix64 totalPushForce = 0;
foreach (HitBox hitBox in hitBoxes)
{
if (hitBox.collisionType != CollisionType.bodyCollider)
{
continue;
}
foreach (HitBox opHitBox in opHitBoxes)
{
if (opHitBox.collisionType != CollisionType.bodyCollider)
{
continue;
}
FPVector opHitBoxPosition = opHitBox.mappedPosition + opRootPosition;
FPVector hitBoxPosition = hitBox.mappedPosition + myRootPosition;
if (!UFE.config.detect3D_Hits)
{
opHitBoxPosition.z = 0;
hitBoxPosition.z = 0;
}
Fix64 dist = FPVector.Distance(opHitBoxPosition, hitBoxPosition);
if (dist <= opHitBox._radius + hitBox._radius)
{
totalPushForce += (opHitBox._radius + hitBox._radius) - dist;
}
}
}
return(totalPushForce);
}
/// <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 FPMatrix4x4 RotateY(FP radians, FPVector centerPoint)
{
FPMatrix4x4 result;
FP c = FPMath.Cos(radians);
FP s = FPMath.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);
}
public void UpdateMap(int frame)
{
if (controlsScript == null)
{
return;
}
if (animationMaps == null && controlsScript.myInfo.useAnimationMaps)
{
Debug.LogError("Animation '" + moveSetScript.GetCurrentClipName() + "' has no animation maps");
return;
}
if (controlsScript.myInfo.useAnimationMaps)
{
HitBoxMap[] hitBoxMaps = new HitBoxMap[0];
int highestFrame = 0;
foreach (AnimationMap map in animationMaps)
{
if (map.frame > highestFrame)
{
highestFrame = map.frame;
}
if (map.frame == frame)
{
hitBoxMaps = map.hitBoxMaps;
deltaPosition = map.deltaDisplacement;
break;
}
}
// If frame can't be found, cast the highest possible frame
if (hitBoxMaps.Length == 0)
{
hitBoxMaps = animationMaps[highestFrame].hitBoxMaps;
deltaPosition = animationMaps[highestFrame].deltaDisplacement;
}
foreach (HitBoxMap map in hitBoxMaps)
{
foreach (HitBox hitBox in hitBoxes)
{
if (hitBox.bodyPart == map.bodyPart)
{
hitBox.mappedPosition = map.mappedPosition;
if (currentMirror)
{
hitBox.mappedPosition.x += (hitBox.mappedPosition.x * -2);
}
}
}
}
}
else
{
foreach (HitBox hitBox in hitBoxes)
{
hitBox.mappedPosition = FPVector.ToFPVector(hitBox.position.position) - worldTransform.position;
}
}
}
/// <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>
/// <returns>The resulting rotation matrix</returns>
public static FPMatrix4x4 AngleAxis(FP angle, FPVector axis)
{
FPMatrix4x4 result;
AxisAngle(ref axis, angle, out result);
return(result);
}
public ConstraintHierarchy(IBody parent, IBody child, FPVector childOffset) : base((RigidBody)parent, (RigidBody)child)
{
this.parent = (RigidBody)parent;
this.child = (RigidBody)child;
this.childOffset = childOffset;
}