public FP.Vector pos; // where to move to
#endregion Fields
#region Constructors
public MoveCmdEvt(long timeVal, long timeCmdVal, Dictionary<int, int[]> pathsVal, FP.Vector posVal, Formation formationVal, bool autoTimeTravelVal)
: base(timeVal, timeCmdVal, pathsVal)
{
pos = posVal;
formation = formationVal;
autoTimeTravel = autoTimeTravelVal;
}
public FP.Vector vecStart; // if rotation is later implemented, can store it in z value
#endregion Fields
#region Constructors
public Move(long timeStartVal, long timeEndVal, FP.Vector vecStartVal, FP.Vector vecEndVal)
{
timeStart = timeStartVal;
timeEnd = timeEndVal;
vecStart = vecStartVal;
vecEnd = vecEndVal;
}
public MakeUnitCmdEvt(long timeVal, long timeCmdVal, Dictionary<int, int[]> pathsVal, int typeVal, FP.Vector posVal, bool autoTimeTravelVal, bool autoRepeatVal = false)
: base(timeVal, timeCmdVal, pathsVal)
{
type = typeVal;
pos = posVal;
autoTimeTravel = autoTimeTravelVal;
autoRepeat = autoRepeatVal;
}
public long timeSimPast; // time traveling simulation time if made in the past, otherwise set to long.MaxValue
#endregion Fields
#region Constructors
public Path(Sim simVal, int idVal, List<Unit> units, long startTime, FP.Vector startPos, bool startUnseen, int nSeeUnitsVal, int startTileXVal, int startTileYVal)
{
g = simVal;
id = idVal;
speed = units[0].type.speed;
player = units[0].player;
segments = new List<Segment> {
new Segment(this, 0, startTime, units, startUnseen)
};
moves = new List<Move> {
new Move(startTime, startPos)
};
nSeeUnits = nSeeUnitsVal;
startTileX = startTileXVal;
startTileY = startTileYVal;
tileX = Sim.offMap + 1;
tileY = Sim.offMap + 1;
timeSimPast = (startTime >= g.timeSim) ? long.MaxValue : startTime / g.tileInterval * g.tileInterval;
}
public override FPVector2 GetReactionForce(FP invDt)
{
FPVector2 P = _impulse * _JvAC;
return(invDt * P);
}
/// <summary>
/// Integrates the whole world a timestep further in time.
/// </summary>
/// <param name="timestep">The timestep in seconds.
/// It should be small as possible to keep the simulation stable.
/// The physics simulation shouldn't run slower than 60fps.
/// (timestep=1/60).</param>
public void Step(FP timestep)
{
this.timestep = timestep;
// yeah! nothing to do!
if (timestep == FP.Zero)
{
return;
}
// throw exception if the timestep is smaller zero.
if (timestep < FP.Zero)
{
throw new ArgumentException("The timestep can't be negative.", "timestep");
}
// Calculate this
//currentAngularDampFactor = (FP)Math.Pow((double)(float)angularDamping, (double)(float)timestep);
//currentLinearDampFactor = (FP)Math.Pow((double)(float)linearDamping, (double)(float)timestep);
#if (WINDOWS_PHONE)
events.RaiseWorldPreStep(timestep);
foreach (RigidBody body in rigidBodies)
{
body.PreStep(timestep);
}
UpdateContacts();
while (removedArbiterQueue.Count > 0)
{
islands.ArbiterRemoved(removedArbiterQueue.Dequeue());
}
foreach (SoftBody body in softbodies)
{
body.Update(timestep);
body.DoSelfCollision(collisionDetectionHandler);
}
CollisionSystem.Detect();
while (addedArbiterQueue.Count > 0)
{
islands.ArbiterCreated(addedArbiterQueue.Dequeue());
}
CheckDeactivation();
IntegrateForces();
HandleArbiter(contactIterations);
Integrate();
foreach (RigidBody body in rigidBodies)
{
body.PostStep(timestep);
}
events.RaiseWorldPostStep(timestep);
#else
events.RaiseWorldPreStep(timestep);
UpdateContacts();
for (int index = 0, length = initialCollisions.Count; index < length; index++)
{
OverlapPairContact op = initialCollisions[index];
events.RaiseBodiesStayCollide(op.contact);
}
for (int index = 0, length = initialTriggers.Count; index < length; index++)
{
OverlapPairContact op = initialTriggers[index];
events.RaiseTriggerStayCollide(op.contact);
}
while (removedArbiterQueue.Count > 0)
{
islands.ArbiterRemoved(removedArbiterQueue.Dequeue());
}
for (int index = 0, length = softbodies.Count; index < length; index++)
{
SoftBody body = softbodies[index];
body.Update(timestep);
body.DoSelfCollision(collisionDetectionHandler);
}
CollisionSystem.Detect();
while (addedArbiterQueue.Count > 0)
{
islands.ArbiterCreated(addedArbiterQueue.Dequeue());
}
CheckDeactivation();
IntegrateForces(); //作用力
HandleArbiter(contactIterations);
Integrate();
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
RigidBody body = rigidBodies[index];
body.PostStep();
for (int index2 = 0, length2 = body.constraints.Count; index2 < length2; index2++)
{
body.constraints[index2].PostStep();
}
}
events.RaiseWorldPostStep(timestep);
#endif
}
void Awake()
{
m_code_rep_movs_copy = new byte[] { 0x8B, 0x4C, 0x24, 0x0C, 0xC1, 0xF9, 0x02, 0x56, 0x8B, 0x74, 0x24, 0x0C, 0x57, 0x8B, 0x7C, 0x24, 0x0C, 0x83, 0xC6, 0x10, 0x83, 0xC7, 0x10, 0xF3, 0xA5, 0x5F, 0x5E, 0xC3 };
rep_movs_copy = (FP)Marshal.GetDelegateForFunctionPointer(Marshal.UnsafeAddrOfPinnedArrayElement(m_code_rep_movs_copy, 0), typeof(FP));
m_code_sse_copy = new byte[] { 0x83, 0xEC, 0x10, 0x8B, 0x4C, 0x24, 0x14, 0xBA, 0x10, 0x00, 0x00, 0x00, 0x56, 0x57, 0x8B, 0x7C, 0x24, 0x20, 0x83, 0xC1, 0x10, 0x83, 0xC7, 0x10, 0x89, 0x4C, 0x24, 0x1C, 0x8D, 0x71, 0x0F, 0x8D, 0x47, 0x0F, 0x83, 0xE6, 0xF0, 0x83, 0xE0, 0xF0, 0x89, 0x74, 0x24, 0x14, 0x89, 0x44, 0x24, 0x10, 0x33, 0xC0, 0x3B, 0xF1, 0x0F, 0x45, 0xC2, 0x8B, 0x54, 0x24, 0x24, 0x2B, 0xD0, 0xC1, 0xEA, 0x07, 0x89, 0x54, 0x24, 0x08, 0x85, 0xD2, 0x0F, 0x84, 0x97, 0x00, 0x00, 0x00, 0x8B, 0x44, 0x24, 0x10, 0x8D, 0x4E, 0x30, 0x89, 0x74, 0x24, 0x20, 0x83, 0xC0, 0x20, 0x8B, 0x74, 0x24, 0x10, 0x29, 0x74, 0x24, 0x20, 0x8B, 0x74, 0x24, 0x14, 0x89, 0x54, 0x24, 0x0C, 0x8B, 0x54, 0x24, 0x20, 0x8B, 0xFF, 0xFF, 0x4C, 0x24, 0x0C, 0x8D, 0x80, 0x80, 0x00, 0x00, 0x00, 0x66, 0x0F, 0x6F, 0x88, 0x70, 0xFF, 0xFF, 0xFF, 0x8D, 0x89, 0x80, 0x00, 0x00, 0x00, 0x66, 0x0F, 0x6F, 0x50, 0x80, 0x66, 0x0F, 0x6F, 0x58, 0x90, 0x66, 0x0F, 0x6F, 0x80, 0x60, 0xFF, 0xFF, 0xFF, 0x66, 0x0F, 0x7F, 0x81, 0x50, 0xFF, 0xFF, 0xFF, 0x66, 0x0F, 0x7F, 0x89, 0x60, 0xFF, 0xFF, 0xFF, 0x66, 0x0F, 0x7F, 0x54, 0x02, 0x80, 0x66, 0x0F, 0x7F, 0x59, 0x80, 0x66, 0x0F, 0x6F, 0x48, 0xB0, 0x66, 0x0F, 0x6F, 0x50, 0xC0, 0x66, 0x0F, 0x6F, 0x58, 0xD0, 0x66, 0x0F, 0x6F, 0x40, 0xA0, 0x66, 0x0F, 0x7F, 0x41, 0x90, 0x66, 0x0F, 0x7F, 0x49, 0xA0, 0x66, 0x0F, 0x7F, 0x51, 0xB0, 0x66, 0x0F, 0x7F, 0x59, 0xC0, 0x75, 0x91, 0x8B, 0x4C, 0x24, 0x1C, 0x2B, 0x74, 0x24, 0x1C, 0x33, 0xD2, 0x8B, 0x44, 0x24, 0x14, 0x83, 0xC6, 0x03, 0xC1, 0xEE, 0x02, 0x39, 0x44, 0x24, 0x1C, 0x0F, 0x47, 0xF2, 0x85, 0xF6, 0x74, 0x16, 0x2B, 0x7C, 0x24, 0x1C, 0x8B, 0x04, 0x0F, 0x8D, 0x49, 0x04, 0x42, 0x89, 0x41, 0xFC, 0x3B, 0xD6, 0x72, 0xF2, 0x8B, 0x44, 0x24, 0x14, 0x8B, 0x4C, 0x24, 0x08, 0x33, 0xD2, 0x8B, 0x7C, 0x24, 0x10, 0xC1, 0xE1, 0x07, 0x03, 0xC1, 0x03, 0xF9, 0x8B, 0x4C, 0x24, 0x1C, 0x03, 0x4C, 0x24, 0x24, 0x8B, 0xF1, 0x2B, 0xF0, 0x83, 0xC6, 0x03, 0xC1, 0xEE, 0x02, 0x3B, 0xC1, 0x0F, 0x47, 0xF2, 0x85, 0xF6, 0x74, 0x10, 0x2B, 0xF8, 0x8B, 0x0C, 0x07, 0x8D, 0x40, 0x04, 0x42, 0x89, 0x48, 0xFC, 0x3B, 0xD6, 0x72, 0xF2, 0x5F, 0x5E, 0x83, 0xC4, 0x10, 0xC3 };
sse_copy = (FP)Marshal.GetDelegateForFunctionPointer(Marshal.UnsafeAddrOfPinnedArrayElement(m_code_sse_copy, 0), typeof(FP));
Test("PlainCopy", () => { PlainCopy(); });
Test("RepMovsCopy", () => { RepMovsCopy(); });
Test("SSECopy", () => { SSECopy(); });
}
public override FP ComputeSubmergedArea(ref TSVector2 normal, FP offset, ref Transform xf, out TSVector2 sc)
{
sc = TSVector2.zero;
return(0);
}
/// <summary> /// Get the reaction force on body at the joint anchor in Newtons. /// </summary> /// <param name="invDt">The inverse delta time.</param> public abstract FPVector2 GetReactionForce(FP invDt);
/// <returns>path that was moved (might not be original path)</returns>
public Path moveTo(long time, List<Unit> units, FP.Vector pos, bool autoTimeTravel)
{
Path movedPath;
FP.Vector goalPos = pos;
// don't move off map edge
if (goalPos.x < 0) goalPos.x = 0;
if (goalPos.x > g.mapSize) goalPos.x = g.mapSize;
if (goalPos.y < 0) goalPos.y = 0;
if (goalPos.y > g.mapSize) goalPos.y = g.mapSize;
if (autoTimeTravel && units.Find (u => {
Waypoint waypoint = g.tileAt (goalPos).waypointWhen (u, time);
return !Waypoint.active (waypoint) || Move.fromSpeed (waypoint.time, speed, waypoint.tile.centerPos (), goalPos).timeEnd > time;
}) == null) {
// move units with automatic time travel
List<Path> movedPaths = new List<Path>();
long stackTime = long.MinValue;
foreach (Unit unit in units) {
Waypoint waypoint = g.tileAt (goalPos).waypointWhen (unit, time);
// make moves list using waypoints
List<Move> waypointMoves = new List<Move> {
Move.fromSpeed (waypoint.time, speed, waypoint.tile.centerPos (), goalPos)
};
while (waypoint.prev != null) {
waypointMoves.Insert (0, new Move(waypoint.time - (waypointMoves[0].vecStart - waypoint.prev.tile.centerPos()).length () / speed,
waypoint.time, waypoint.prev.tile.centerPos (), waypointMoves[0].vecStart));
waypoint = waypoint.prev;
}
waypointMoves.Insert (0, new Move(waypoint.start[0].time, waypoint.time, waypoint.start[0].path.posWhen (waypoint.start[0].time), waypointMoves[0].vecStart));
// do path smoothing
for (int i = 0; i < waypointMoves.Count; i++) {
int j;
for (j = i + 1; j < waypointMoves.Count; j++) {
Move move = Move.fromSpeed(waypointMoves[i].timeStart, speed, waypointMoves[i].vecStart, waypointMoves[j].vecEnd);
long timeMove = (waypointMoves[i].timeStart / g.tileInterval + 1) * g.tileInterval;
while (timeMove < waypointMoves[j].timeEnd && g.tileAt(move.posWhen(timeMove)).exclusiveWhen(player, timeMove)) {
timeMove += g.tileInterval;
}
if (timeMove < waypointMoves[j].timeEnd) break;
}
j--;
if (j > i) {
waypointMoves[i] = Move.fromSpeed(waypointMoves[i].timeStart, speed, waypointMoves[i].vecStart, waypointMoves[j].vecEnd);
waypointMoves.RemoveRange(i + 1, j - i);
}
}
// if unit not found on start waypoint, add it back to past segments
for (int i = 0; i < waypoint.start.Count - 1; i++) {
Segment segment = waypoint.start[i + 1].path.insertSegment (waypoint.start[i].time);
while (segment.timeStart != waypoint.start[i + 1].time) {
segment = segment.prevOnPath ();
if (segment.units.Contains (unit)) {
i = waypoint.start.Count;
break;
}
segment.units.Add (unit);
segment.deletedUnits.Remove(unit);
}
}
// make non-live path moving along waypoints
if (!waypoint.start[0].segment().path.makePath (waypointMoves[0].timeStart, new List<Unit> { unit })) {
throw new SystemException("make auto time travel path failed when moving units");
}
g.paths.Last ().moves = waypointMoves;
g.paths.Last ().updatePast(time, false);
// add kept unit line
MoveLine keepLine = new MoveLine(time, player);
keepLine.vertices.AddRange (g.paths.Last ().moveLines (waypointMoves[0].timeStart, time));
g.keepLines.Add (keepLine);
g.alternatePaths.Add (g.paths.Last ());
movedPaths.Add (g.paths.Last ());
stackTime = Math.Max (stackTime, waypointMoves.Last ().timeEnd);
}
player.updatePast (time);
if (units.Count > 1) new StackEvt(stackTime, movedPaths, nSeeUnits).apply (g);
movedPath = movedPaths.Find (p => p.segments.Last ().units.Count > 0);
} else {
// move units normally (without automatic time travel)
movedPath = this; // move this path by default
if (time < g.timeSim) {
// move non-live path if in past
// if this path already isn't live, a better approach might be removing later segments and moves then moving this path, like pre-stacking versions (see ISSUE #27)
if (!makePath (time, units)) throw new SystemException("make non-live path failed when moving units");
movedPath = g.paths.Last ();
} else {
foreach (Unit unit in segmentWhen(time).units) {
if (!units.Contains (unit)) {
// some units in path aren't being moved, so make a new path
if (!makePath (time, units, true)) throw new SystemException("make new path failed when moving units");
movedPath = g.paths.Last ();
break;
}
}
}
movedPath.moveToDirect (time, pos);
if (movedPath != this) g.alternatePaths.Add (movedPath);
}
if (movedPath != this && (movedPath.timeSimPast == long.MaxValue || timeSimPast != long.MaxValue)) {
// new path was moved, so try to remove units that moved from current path
Segment segment = segmentWhen (time);
foreach (Unit unit in units) {
new SegmentUnit(segment, unit).delete ();
}
}
return movedPath;
}
private Vector3 simToDrawPos(FP.Vector vec, float depth = 0f)
{
return new Vector3(simToDrawScl(vec.x - g.camPos.x) + Screen.width / 2, simToDrawScl(vec.y - g.camPos.y) + Screen.height / 2, depth);
}
public override FPVector2 GetReactionForce(FP invDt)
{
return(invDt * _impulse);
}
private void FillAttractTimeData(int i_SlotIndex, tnStandardMatchController i_MatchController)
{
if (i_MatchController == null)
{
return;
}
int charcatersCount = i_MatchController.charactersCount;
// Compute total shots count.
FP totalAttractTime = FP.Zero;
for (int characterIndex = 0; characterIndex < charcatersCount; ++characterIndex)
{
tnStandardMatchCharacterResults characterResults = (tnStandardMatchCharacterResults)i_MatchController.GetCharacterResultsByIndex(characterIndex);
if (characterResults != null)
{
totalAttractTime += characterResults.attractTime;
}
}
// Get best character for this stat.
int selectedCharacterIndex = -1;
int selectedCharacterId = Hash.s_NULL;
FP maxAttractTime = FP.MinValue;
for (int characterIndex = 0; characterIndex < charcatersCount; ++characterIndex)
{
tnStandardMatchCharacterResults characterResults = (tnStandardMatchCharacterResults)i_MatchController.GetCharacterResultsByIndex(characterIndex);
if (characterResults != null)
{
FP characterAttractTime = characterResults.attractTime;
if (characterAttractTime > maxAttractTime)
{
selectedCharacterIndex = characterIndex;
selectedCharacterId = characterResults.id;
maxAttractTime = characterAttractTime;
}
}
}
if (selectedCharacterIndex < 0)
{
return;
}
// Get team color.
Color teamColor = Color.white;
GameObject characterGo = i_MatchController.GetCharacterByIndex(selectedCharacterIndex);
if (characterGo != null)
{
tnCharacterInfo characterInfo = characterGo.GetComponent <tnCharacterInfo>();
if (characterInfo != null)
{
teamColor = characterInfo.teamColor;
}
}
// Fill data.
string playerName = "";
Sprite playerPortrait = null;
{
tnCharacterData characterData = tnGameData.GetCharacterDataMain(selectedCharacterId);
if (characterData != null)
{
playerName = characterData.displayName;
playerPortrait = characterData.uiIconFacingRight;
}
}
string statValue = maxAttractTime.ToString(2);
statValue += " s";
float partecipationPercentage = 0f;
if (totalAttractTime > 0f)
{
partecipationPercentage = maxAttractTime.AsFloat() / totalAttractTime.AsFloat();
partecipationPercentage *= 100f;
partecipationPercentage = Mathf.Clamp(partecipationPercentage, 0f, 100f);
}
string partecipationValue = partecipationPercentage.ToString("0.00");
partecipationValue += "%";
FillData(i_SlotIndex, playerName, playerPortrait, teamColor, s_StatName_AttractTime, statValue, s_PartecipationLabel, partecipationValue);
}
public void moveToDirect(long time, FP.Vector pos)
{
FP.Vector curPos = posWhen(time);
FP.Vector goalPos = pos;
// don't move off map edge
if (goalPos.x < 0) goalPos.x = 0;
if (goalPos.x > g.mapSize) goalPos.x = g.mapSize;
if (goalPos.y < 0) goalPos.y = 0;
if (goalPos.y > g.mapSize) goalPos.y = g.mapSize;
// add move
moves.Add (Move.fromSpeed(time, speed, curPos, goalPos));
}
Pair <OrderedSequence <T, M>, OrderedSequence <T, M> > Partition(M vK)
{
Pair <FTreeM <OrderedElement <T, M>, M>, FTreeM <OrderedElement <T, M>, M> > baseSeqSplit = _tree.SeqSplit(new MPredicate <M>(FP.Curry <M, M, bool>(LessThanOrEqual2, vK)));
var left = new OrderedSequence <T, M>(_key, baseSeqSplit.First);
var right = new OrderedSequence <T, M>(_key, baseSeqSplit.Second);
return(new Pair <OrderedSequence <T, M>, OrderedSequence <T, M> >(left, right));
}
public static List <Fixture> AttachSolidArc(FP density, FP radians, int sides, FP radius, FPVector2 position, FP angle, Body body)
{
Vertices arc = PolygonTools.CreateArc(radians, sides, radius);
arc.Rotate((FP.Pi - radians) / 2 + angle);
arc.Translate(ref position);
//Close the arc
arc.Add(arc[0]);
List <Vertices> triangles = Triangulate.ConvexPartition(arc, TriangulationAlgorithm.Earclip, true, FP.EN3);
return(AttachCompoundPolygon(triangles, density, body));
}
public static Fixture AttachLineArc(FP radians, int sides, FP radius, FPVector2 position, FP angle, bool closed, Body body)
{
Vertices arc = PolygonTools.CreateArc(radians, sides, radius);
arc.Rotate((FP.Pi - radians) / 2 + angle);
arc.Translate(ref position);
return(closed ? AttachLoopShape(arc, body) : AttachChainShape(arc, body));
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = _bodyA.IslandIndex;
_indexB = _bodyB.IslandIndex;
_indexC = _bodyC.IslandIndex;
_indexD = _bodyD.IslandIndex;
_lcA = _bodyA._sweep.LocalCenter;
_lcB = _bodyB._sweep.LocalCenter;
_lcC = _bodyC._sweep.LocalCenter;
_lcD = _bodyD._sweep.LocalCenter;
_mA = _bodyA._invMass;
_mB = _bodyB._invMass;
_mC = _bodyC._invMass;
_mD = _bodyD._invMass;
_iA = _bodyA._invI;
_iB = _bodyB._invI;
_iC = _bodyC._invI;
_iD = _bodyD._invI;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
FP aC = data.positions[_indexC].a;
FPVector2 vC = data.velocities[_indexC].v;
FP wC = data.velocities[_indexC].w;
FP aD = data.positions[_indexD].a;
FPVector2 vD = data.velocities[_indexD].v;
FP wD = data.velocities[_indexD].w;
Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
_mass = 0.0f;
if (_typeA == JointType.Revolute)
{
_JvAC = FPVector2.zero;
_JwA = 1.0f;
_JwC = 1.0f;
_mass += _iA + _iC;
}
else
{
FPVector2 u = MathUtils.Mul(qC, _localAxisC);
FPVector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
FPVector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
_JvAC = u;
_JwC = MathUtils.Cross(rC, u);
_JwA = MathUtils.Cross(rA, u);
_mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA;
}
if (_typeB == JointType.Revolute)
{
_JvBD = FPVector2.zero;
_JwB = _ratio;
_JwD = _ratio;
_mass += _ratio * _ratio * (_iB + _iD);
}
else
{
FPVector2 u = MathUtils.Mul(qD, _localAxisD);
FPVector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
FPVector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
_JvBD = _ratio * u;
_JwD = _ratio * MathUtils.Cross(rD, u);
_JwB = _ratio * MathUtils.Cross(rB, u);
_mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB;
}
// Compute effective mass.
_mass = _mass > 0.0f ? 1.0f / _mass : 0.0f;
if (Settings.EnableWarmstarting)
{
vA += (_mA * _impulse) * _JvAC;
wA += _iA * _impulse * _JwA;
vB += (_mB * _impulse) * _JvBD;
wB += _iB * _impulse * _JwB;
vC -= (_mC * _impulse) * _JvAC;
wC -= _iC * _impulse * _JwC;
vD -= (_mD * _impulse) * _JvBD;
wD -= _iD * _impulse * _JwD;
}
else
{
_impulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
data.velocities[_indexC].v = vC;
data.velocities[_indexC].w = wC;
data.velocities[_indexD].v = vD;
data.velocities[_indexD].w = wD;
}
/// <summary>
/// Gets the motor force.
/// </summary>
/// <param name="invDt">The inverse delta time</param>
public FP GetMotorForce(FP invDt)
{
return(invDt * MotorImpulse);
}
public override FP GetReactionTorque(FP invDt)
{
return(invDt * 0.0f);
}
private FP.Vector jsonFPVector(Hashtable json, string key, FP.Vector defaultVal = new FP.Vector())
{
if (json.ContainsKey(key) && json[key] is Hashtable) {
return new FP.Vector(jsonFP((Hashtable)json[key], "x", defaultVal.x),
jsonFP((Hashtable)json[key], "y", defaultVal.y),
jsonFP((Hashtable)json[key], "z", defaultVal.z));
}
return defaultVal;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invIA = BodyA._invI;
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
Rot qA = new Rot(aA);
FP mass = BodyA.Mass;
// Frequency
FP omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
FP d = 2.0f * mass * DampingRatio * omega;
// Spring stiffness
FP k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
FP h = data.step.dt;
//Debug.Assert(d + h * k > Settings.Epsilon);
_gamma = h * (d + h * k);
if (_gamma != 0.0f)
{
_gamma = 1.0f / _gamma;
}
_beta = h * k * _gamma;
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
// [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X]
Mat22 K = new Mat22();
K.ex.x = _invMassA + _invIA * _rA.y * _rA.y + _gamma;
K.ex.y = -_invIA * _rA.x * _rA.y;
K.ey.x = K.ex.y;
K.ey.y = _invMassA + _invIA * _rA.x * _rA.x + _gamma;
_mass = K.Inverse;
_C = cA + _rA - _worldAnchor;
_C *= _beta;
// Cheat with some damping
wA *= 0.98f;
if (Settings.EnableWarmstarting)
{
_impulse *= data.step.dtRatio;
vA += _invMassA * _impulse;
wA += _invIA * MathUtils.Cross(_rA, _impulse);
}
else
{
_impulse = FPVector2.zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
}
public void FrameUpdate(FP deltaTime)
{
}
public override FPVector2 GetReactionForce(FP invDt)
{
return(invDt * new FPVector2(_impulse.x, _impulse.y));
}
/// <summary> /// Get the reaction torque on the body at the joint anchor in N*m. /// </summary> /// <param name="invDt">The inverse delta time.</param> public abstract FP GetReactionTorque(FP invDt);
public Move(long timeVal, FP.Vector vecVal)
: this(timeVal, timeVal + 1, vecVal, vecVal)
{
}
public override FP GetReactionTorque(FP invDt)
{
return(invDt * _impulse.z);
}
public Tile tileAt(FP.Vector pos)
{
return tiles[pos.x >> FP.precision, pos.y >> FP.precision];
}
/// <summary>
/// alternate method to create Move object that asks for speed (in position units per millisecond) instead of end time
/// </summary>
public static Move fromSpeed(long timeStartVal, long speed, FP.Vector vecStartVal, FP.Vector vecEndVal)
{
return new Move(timeStartVal, timeStartVal + (vecEndVal - vecStartVal).length() / speed, vecStartVal, vecEndVal);
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
Mat33 K = new Mat33();
K.ex.x = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB;
K.ey.x = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB;
K.ez.x = -_rA.y * iA - _rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB;
K.ez.y = _rA.x * iA + _rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (FrequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
FP invM = iA + iB;
FP m = invM > 0.0f ? 1.0f / invM : 0.0f;
FP C = aB - aA - ReferenceAngle;
// Frequency
FP omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
FP d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
FP k = m * omega * omega;
// magic formulas
FP h = data.step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invM += _gamma;
_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
FPVector2 P = new FPVector2(_impulse.x, _impulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _impulse.z);
}
else
{
_impulse = FPVector.zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
private void CollisionDetected(RigidBody body1, RigidBody body2, TSVector point1, TSVector point2, TSVector normal, FP penetration)
{
bool anyBodyColliderOnly = body1.IsColliderOnly || body2.IsColliderOnly;
Arbiter arbiter = null;
ArbiterMap selectedArbiterMap = null;
if (anyBodyColliderOnly)
{
selectedArbiterMap = arbiterTriggerMap;
}
else
{
selectedArbiterMap = arbiterMap;
}
bool arbiterCreated = false;
lock (selectedArbiterMap) {
selectedArbiterMap.LookUpArbiter(body1, body2, out arbiter);
if (arbiter == null)
{
arbiter = Arbiter.Pool.GetNew();
arbiter.body1 = body1; arbiter.body2 = body2;
selectedArbiterMap.Add(new ArbiterKey(body1, body2), arbiter);
arbiterCreated = true;
}
}
Contact contact = null;
if (arbiter.body1 == body1)
{
TSVector.Negate(ref normal, out normal);
contact = arbiter.AddContact(point1, point2, normal, penetration, contactSettings);
}
else
{
contact = arbiter.AddContact(point2, point1, normal, penetration, contactSettings);
}
if (arbiterCreated)
{
if (anyBodyColliderOnly)
{
/*if (body1.isColliderOnly) {
* events.RaiseTriggerBeginCollide(body1, body2);
* } else {
* events.RaiseTriggerBeginCollide(body2, body1);
* }*/
events.RaiseTriggerBeginCollide(contact);
body1.arbitersTrigger.Add(arbiter);
body2.arbitersTrigger.Add(arbiter);
OverlapPairContact overlapContact = new OverlapPairContact(body1, body2);
overlapContact.contact = contact;
initialTriggers.Add(overlapContact);
}
else
{
events.RaiseBodiesBeginCollide(contact);
addedArbiterQueue.Enqueue(arbiter);
OverlapPairContact overlapContact = new OverlapPairContact(body1, body2);
overlapContact.contact = contact;
initialCollisions.Add(overlapContact);
}
}
if (!anyBodyColliderOnly && contact != null)
{
events.RaiseContactCreated(contact);
}
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FP positionError, angularError;
Mat33 K = new Mat33();
K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
K.ez.x = -rA.y * iA - rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
K.ez.y = rA.x * iA + rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (FrequencyHz > 0.0f)
{
FPVector2 C1 = cB + rB - cA - rA;
positionError = C1.magnitude;
angularError = 0.0f;
FPVector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
FPVector2 C1 = cB + rB - cA - rA;
FP C2 = aB - aA - ReferenceAngle;
positionError = C1.magnitude;
angularError = FP.Abs(C2);
FPVector C = new FPVector(C1.x, C1.y, C2);
FPVector impulse = K.Solve33(C) * -1;
FPVector2 P = new FPVector2(impulse.x, impulse.y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.z);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
/// <summary>
/// Requires two existing revolute or prismatic joints (any combination will work).
/// The provided joints must attach a dynamic body to a static body.
/// </summary>
/// <param name="jointA">The first joint.</param>
/// <param name="jointB">The second joint.</param>
/// <param name="ratio">The ratio.</param>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
// TS - public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, FP ratio = 1f)
public GearJoint(Body bodyA, Body bodyB, Joint2D jointA, Joint2D jointB, FP ratio)
{
JointType = JointType.Gear;
BodyA = bodyA;
BodyB = bodyB;
JointA = jointA;
JointB = jointB;
Ratio = ratio;
_typeA = jointA.JointType;
_typeB = jointB.JointType;
Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic);
Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic);
FP coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
_bodyC = JointA.BodyA;
_bodyA = JointA.BodyB;
// Get geometry of joint1
Transform xfA = _bodyA._xf;
FP aA = _bodyA._sweep.A;
Transform xfC = _bodyC._xf;
FP aC = _bodyC._sweep.A;
if (_typeA == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointA;
_localAnchorC = revolute.LocalAnchorA;
_localAnchorA = revolute.LocalAnchorB;
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC = FPVector2.zero;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointA;
_localAnchorC = prismatic.LocalAnchorA;
_localAnchorA = prismatic.LocalAnchorB;
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalXAxis;
FPVector2 pC = _localAnchorC;
FPVector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
coordinateA = FPVector2.Dot(pA - pC, _localAxisC);
}
_bodyD = JointB.BodyA;
_bodyB = JointB.BodyB;
// Get geometry of joint2
Transform xfB = _bodyB._xf;
FP aB = _bodyB._sweep.A;
Transform xfD = _bodyD._xf;
FP aD = _bodyD._sweep.A;
if (_typeB == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointB;
_localAnchorD = revolute.LocalAnchorA;
_localAnchorB = revolute.LocalAnchorB;
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD = FPVector2.zero;
coordinateB = aB - aD - _referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointB;
_localAnchorD = prismatic.LocalAnchorA;
_localAnchorB = prismatic.LocalAnchorB;
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalXAxis;
FPVector2 pD = _localAnchorD;
FPVector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
coordinateB = FPVector2.Dot(pB - pD, _localAxisD);
}
_ratio = ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}
/// <summary>
/// Initializes a new instance of the matrix structure.
/// </summary>
/// <param name="m11">m11</param>
/// <param name="m12">m12</param>
/// <param name="m13">m13</param>
/// <param name="m21">m21</param>
/// <param name="m22">m22</param>
/// <param name="m23">m23</param>
/// <param name="m31">m31</param>
/// <param name="m32">m32</param>
/// <param name="m33">m33</param>
#region public JMatrix(FP m11, FP m12, FP m13, FP m21, FP m22, FP m23,FP m31, FP m32, FP m33)
public TSMatrix(FP m11, FP m12, FP m13, FP m21, FP m22, FP m23, FP m31, FP m32, FP m33)
{
this.M11 = m11;
this.M12 = m12;
this.M13 = m13;
this.M21 = m21;
this.M22 = m22;
this.M23 = m23;
this.M31 = m31;
this.M32 = m32;
this.M33 = m33;
}
public override FP GetReactionTorque(FP invDt)
{
FP L = _impulse * _JwA;
return(invDt * L);
}
/// <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 TSMatrix AngleAxis(FP angle, TSVector axis)
{
TSMatrix result; CreateFromAxisAngle(ref axis, angle, out result);
return(result);
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
FPVector2 cC = data.positions[_indexC].c;
FP aC = data.positions[_indexC].a;
FPVector2 cD = data.positions[_indexD].c;
FP aD = data.positions[_indexD].a;
Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
FP linearError = 0.0f;
FP coordinateA, coordinateB;
FPVector2 JvAC, JvBD;
FP JwA, JwB, JwC, JwD;
FP mass = 0.0f;
if (_typeA == JointType.Revolute)
{
JvAC = FPVector2.zero;
JwA = 1.0f;
JwC = 1.0f;
mass += _iA + _iC;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
FPVector2 u = MathUtils.Mul(qC, _localAxisC);
FPVector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
FPVector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
JvAC = u;
JwC = MathUtils.Cross(rC, u);
JwA = MathUtils.Cross(rA, u);
mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA;
FPVector2 pC = _localAnchorC - _lcC;
FPVector2 pA = MathUtils.MulT(qC, rA + (cA - cC));
coordinateA = FPVector2.Dot(pA - pC, _localAxisC);
}
if (_typeB == JointType.Revolute)
{
JvBD = FPVector2.zero;
JwB = _ratio;
JwD = _ratio;
mass += _ratio * _ratio * (_iB + _iD);
coordinateB = aB - aD - _referenceAngleB;
}
else
{
FPVector2 u = MathUtils.Mul(qD, _localAxisD);
FPVector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
FPVector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
JvBD = _ratio * u;
JwD = _ratio * MathUtils.Cross(rD, u);
JwB = _ratio * MathUtils.Cross(rB, u);
mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB;
FPVector2 pD = _localAnchorD - _lcD;
FPVector2 pB = MathUtils.MulT(qD, rB + (cB - cD));
coordinateB = FPVector2.Dot(pB - pD, _localAxisD);
}
FP C = (coordinateA + _ratio * coordinateB) - _constant;
FP impulse = 0.0f;
if (mass > 0.0f)
{
impulse = -C / mass;
}
cA += _mA * impulse * JvAC;
aA += _iA * impulse * JwA;
cB += _mB * impulse * JvBD;
aB += _iB * impulse * JwB;
cC -= _mC * impulse * JvAC;
aC -= _iC * impulse * JwC;
cD -= _mD * impulse * JvBD;
aD -= _iD * impulse * JwD;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
data.positions[_indexC].c = cC;
data.positions[_indexC].a = aC;
data.positions[_indexD].c = cD;
data.positions[_indexD].a = aD;
// TODO_ERIN not implemented
return(linearError < Settings.LinearSlop);
}
/// <summary>
/// Calculates the inverse of a give matrix.
/// </summary>
/// <param name="matrix">The matrix to invert.</param>
/// <param name="result">The inverted JMatrix.</param>
public static void Inverse(ref FPMatrix4x4 matrix, out FPMatrix4x4 result)
{
// -1
// If you have matrix M, inverse Matrix M can compute
//
// -1 1
// M = --------- A
// det(M)
//
// A is adjugate (adjoint) of M, where,
//
// T
// A = C
//
// C is Cofactor matrix of M, where,
// i + j
// C = (-1) * det(M )
// ij ij
//
// [ a b c d ]
// M = [ e f g h ]
// [ i j k l ]
// [ m n o p ]
//
// First Row
// 2 | f g h |
// C = (-1) | j k l | = + ( f ( kp - lo ) - g ( jp - ln ) + h ( jo - kn ) )
// 11 | n o p |
//
// 3 | e g h |
// C = (-1) | i k l | = - ( e ( kp - lo ) - g ( ip - lm ) + h ( io - km ) )
// 12 | m o p |
//
// 4 | e f h |
// C = (-1) | i j l | = + ( e ( jp - ln ) - f ( ip - lm ) + h ( in - jm ) )
// 13 | m n p |
//
// 5 | e f g |
// C = (-1) | i j k | = - ( e ( jo - kn ) - f ( io - km ) + g ( in - jm ) )
// 14 | m n o |
//
// Second Row
// 3 | b c d |
// C = (-1) | j k l | = - ( b ( kp - lo ) - c ( jp - ln ) + d ( jo - kn ) )
// 21 | n o p |
//
// 4 | a c d |
// C = (-1) | i k l | = + ( a ( kp - lo ) - c ( ip - lm ) + d ( io - km ) )
// 22 | m o p |
//
// 5 | a b d |
// C = (-1) | i j l | = - ( a ( jp - ln ) - b ( ip - lm ) + d ( in - jm ) )
// 23 | m n p |
//
// 6 | a b c |
// C = (-1) | i j k | = + ( a ( jo - kn ) - b ( io - km ) + c ( in - jm ) )
// 24 | m n o |
//
// Third Row
// 4 | b c d |
// C = (-1) | f g h | = + ( b ( gp - ho ) - c ( fp - hn ) + d ( fo - gn ) )
// 31 | n o p |
//
// 5 | a c d |
// C = (-1) | e g h | = - ( a ( gp - ho ) - c ( ep - hm ) + d ( eo - gm ) )
// 32 | m o p |
//
// 6 | a b d |
// C = (-1) | e f h | = + ( a ( fp - hn ) - b ( ep - hm ) + d ( en - fm ) )
// 33 | m n p |
//
// 7 | a b c |
// C = (-1) | e f g | = - ( a ( fo - gn ) - b ( eo - gm ) + c ( en - fm ) )
// 34 | m n o |
//
// Fourth Row
// 5 | b c d |
// C = (-1) | f g h | = - ( b ( gl - hk ) - c ( fl - hj ) + d ( fk - gj ) )
// 41 | j k l |
//
// 6 | a c d |
// C = (-1) | e g h | = + ( a ( gl - hk ) - c ( el - hi ) + d ( ek - gi ) )
// 42 | i k l |
//
// 7 | a b d |
// C = (-1) | e f h | = - ( a ( fl - hj ) - b ( el - hi ) + d ( ej - fi ) )
// 43 | i j l |
//
// 8 | a b c |
// C = (-1) | e f g | = + ( a ( fk - gj ) - b ( ek - gi ) + c ( ej - fi ) )
// 44 | i j k |
//
// Cost of operation
// 53 adds, 104 muls, and 1 div.
FP a = matrix.M11, b = matrix.M12, c = matrix.M13, d = matrix.M14;
FP e = matrix.M21, f = matrix.M22, g = matrix.M23, h = matrix.M24;
FP i = matrix.M31, j = matrix.M32, k = matrix.M33, l = matrix.M34;
FP m = matrix.M41, n = matrix.M42, o = matrix.M43, p = matrix.M44;
FP kp_lo = k * p - l * o;
FP jp_ln = j * p - l * n;
FP jo_kn = j * o - k * n;
FP ip_lm = i * p - l * m;
FP io_km = i * o - k * m;
FP in_jm = i * n - j * m;
FP a11 = (f * kp_lo - g * jp_ln + h * jo_kn);
FP a12 = -(e * kp_lo - g * ip_lm + h * io_km);
FP a13 = (e * jp_ln - f * ip_lm + h * in_jm);
FP a14 = -(e * jo_kn - f * io_km + g * in_jm);
FP det = a * a11 + b * a12 + c * a13 + d * a14;
if (det == FP.Zero)
{
result.M11 = FP.PositiveInfinity;
result.M12 = FP.PositiveInfinity;
result.M13 = FP.PositiveInfinity;
result.M14 = FP.PositiveInfinity;
result.M21 = FP.PositiveInfinity;
result.M22 = FP.PositiveInfinity;
result.M23 = FP.PositiveInfinity;
result.M24 = FP.PositiveInfinity;
result.M31 = FP.PositiveInfinity;
result.M32 = FP.PositiveInfinity;
result.M33 = FP.PositiveInfinity;
result.M34 = FP.PositiveInfinity;
result.M41 = FP.PositiveInfinity;
result.M42 = FP.PositiveInfinity;
result.M43 = FP.PositiveInfinity;
result.M44 = FP.PositiveInfinity;
}
else
{
FP invDet = FP.One / det;
result.M11 = a11 * invDet;
result.M21 = a12 * invDet;
result.M31 = a13 * invDet;
result.M41 = a14 * invDet;
result.M12 = -(b * kp_lo - c * jp_ln + d * jo_kn) * invDet;
result.M22 = (a * kp_lo - c * ip_lm + d * io_km) * invDet;
result.M32 = -(a * jp_ln - b * ip_lm + d * in_jm) * invDet;
result.M42 = (a * jo_kn - b * io_km + c * in_jm) * invDet;
FP gp_ho = g * p - h * o;
FP fp_hn = f * p - h * n;
FP fo_gn = f * o - g * n;
FP ep_hm = e * p - h * m;
FP eo_gm = e * o - g * m;
FP en_fm = e * n - f * m;
result.M13 = (b * gp_ho - c * fp_hn + d * fo_gn) * invDet;
result.M23 = -(a * gp_ho - c * ep_hm + d * eo_gm) * invDet;
result.M33 = (a * fp_hn - b * ep_hm + d * en_fm) * invDet;
result.M43 = -(a * fo_gn - b * eo_gm + c * en_fm) * invDet;
FP gl_hk = g * l - h * k;
FP fl_hj = f * l - h * j;
FP fk_gj = f * k - g * j;
FP el_hi = e * l - h * i;
FP ek_gi = e * k - g * i;
FP ej_fi = e * j - f * i;
result.M14 = -(b * gl_hk - c * fl_hj + d * fk_gj) * invDet;
result.M24 = (a * gl_hk - c * el_hi + d * ek_gi) * invDet;
result.M34 = -(a * fl_hj - b * el_hi + d * ej_fi) * invDet;
result.M44 = (a * fk_gj - b * ek_gi + c * ej_fi) * invDet;
}
}
public override FPVector2 GetReactionForce(FP invDt)
{
return(invDt * (_impulse.x * _perp + (MotorImpulse + _impulse.z) * _axis));
}
/// <summary>
/// Creates a uniform scaling matrix that scales equally on each axis.
/// </summary>
/// <param name="scale">The uniform scaling factor.</param>
/// <returns>The scaling matrix.</returns>
public static FPMatrix4x4 Scale(FP scale)
{
return(Scale(scale, scale, scale));
}
private Vector3 simToDrawScl(FP.Vector vec)
{
return new Vector3(simToDrawScl(vec.x), simToDrawScl(vec.y), simToDrawScl(vec.z));
}
/// <summary>
/// Creates a uniform scaling matrix that scales equally on each axis with a center point.
/// </summary>
/// <param name="scale">The uniform scaling factor.</param>
/// <param name="centerPoint">The center point.</param>
/// <returns>The scaling matrix.</returns>
public static FPMatrix4x4 Scale(FP scale, FPVector centerPoint)
{
return(Scale(scale, scale, scale, centerPoint));
}
private void cmdFP_Click(object sender, RoutedEventArgs e)
{
try
{
//dynamic outlook = AutomationFactory.GetObject("Outlook.Application");
////dynamic com = AutomationFactory.CreateObject("Zfpcom.ZekaFP");
////dynamic zfp = AutomationFactory.CreateObject("Zfpcom.ZekaFP");
////zfp.Setup(4, 9600, 3, 1000);
////string sdsd = zfp.GetVersion();
////byte oper = byte.Parse("1");
////zfp.OpenFiscalBon(oper, "0000", 0, 1);
////if (0 != zfp.errorCode)
////{
//// string err = zfp.GetErrorString(zfp.errorCode, 0);
//// MessageBox.Show(err);
////}
////zfp.SellFree("Cooking fat", Convert.ToByte("1"), 5, 3, 0);
////if (0 != zfp.errorCode)
////{
//// string err = zfp.GetErrorString(zfp.errorCode, 0);
//// MessageBox.Show(err);
////}
////zfp.Payment(Single.Parse("200"), Convert.ToByte("0"), 0);
////if (0 != zfp.errorCode)
////{
//// string err = zfp.GetErrorString(zfp.errorCode, 0);
//// MessageBox.Show(err);
////}
////zfp.CloseFiscalBon();
////if (0 != zfp.errorCode)
////{
//// string err = zfp.GetErrorString(zfp.errorCode, 0);
//// MessageBox.Show(err);
////}
//bool isopen = com.Open("COM4");
// string ss = com.Read((char)(3));
//bool isClosed = com.Close();
// dynamic com1 = AutomationFactory.CreateObject("FPRinter");
////string dg= com1.test();
// com1.OpenPort("COM4", 9600);
// //test.PrintLogo();
// //test.TerminateReceipt(false);
// com1.PrintText("Test 1...");
// com1.PrintText("Test Silverlight...");
// com1.ClosePort();
//dynamic count = com.GetDeviceCount();
//StringBuilder sb = new StringBuilder();
//SerialPort c = new SerialPort();
//List<dynamic> devices = new List<dynamic>();
//for (int i = 1; i <= 9; i++)
//{
// devices.Add("COM" + i);
//}
//for (int i = 0; i < count; i++)
//{
// devices.Add(com.GetDevice(i));
//}
//com.device = devices[9];
//com.Open();
//MessageBox.Show(com.GetErrorDescription(com.LastError));
//string buffer = "";
//string tb = "";
//com.WriteLine("dsdsdsdsdsadsad");
//System.Threading.Thread t = new Thread(new ThreadStart(delegate()
// {
// while (1 == 1)
// {
// com.Sleep(200);
// buffer = com.ReadString();
// if (buffer == "") { com.Close(); return; }
// tb += "\r\n" + com.ReadString();
// }
//}));
//t.Start();
//com.Close();
using (FP test = new FP())
{
test.OpenPort("COM4", 9600);
//test.PrintLogo();
//test.TerminateReceipt(false);
test.PrintText("Test 1...");
test.ClosePort();
}
}
catch (Exception ex)
{
if (ex.GetType() == typeof(FPException))
{
int err = ((FPException)ex).ErrorCode;
MessageBox.Show(ex.Message, "Error " + err.ToString(), MessageBoxButton.OK);
}
else
{
MessageBox.Show(ex.Message, "Error", MessageBoxButton.OK);
}
}
}
protected void btnLoadInvoice_Click(object sender, EventArgs e)
{
lblInformationInvoice.Text = "";
string clientBillNumberCell = "";
string idClientCell = "";
string conditionClientCell = "";
string totalClientCell = "";
string dateClientCell = ""; // PARA HACER LA VALIDACION DE SI ES UN ARCHIVO QUE POSEE FACTURAS!!
string dateSupplierCell = "";
string supplierBillNumberCell = "";
string idSupplierCell = "";
string totalSupplierCell = "";
if (FP.PostedFile.ContentLength <= 0)
{
btnUploadInvoice.Visible = false;
lblInformationInvoice.Text = textEmpty;
}
//Get the file extension
string fileExtension = Path.GetExtension(FP.FileName);
//If file is not in excel format then return
if (fileExtension != verifyFileXls && fileExtension != verifyFileXlsx)
{
btnUploadInvoice.Visible = false;
lblInformationInvoice.Text = noExcelFile;
}
else
{
string path = Server.MapPath("~/" + FP.FileName);
//////saving the file inside the MyFolder of the server
FP.SaveAs(path);
using (var stream = File.Open(path, FileMode.Open, FileAccess.Read))
{
using (var reader = ExcelReaderFactory.CreateReader(stream))
{
var result = reader.AsDataSet();
table = result.Tables[0];
btnUploadInvoice.Visible = false;
if (table.Rows[0][1].ToString() == customerTemplateId)
{
// grdInvoice.DataSource = table;
// grdInvoice.DataBind();
ViewState["table"] = table;
isClient = true;
ViewState["isClient"] = isClient;
if (validateInvoice() == true)
{
btnUploadInvoice.Visible = true;
}
}
else
{
if (table.Rows[0][3].ToString() == supplierDate)
{
//grdInvoice.DataSource = table;
//grdInvoice.DataBind();
ViewState["table"] = table;
isClient = false;
ViewState["isClient"] = isClient;
if (validateInvoice() == true)
{
btnUploadInvoice.Visible = true;
}
return;
}
else
{
btnUploadInvoice.Visible = false;
//Response.Write("<script> alert(" + "'El archivo no coincide con el formato de las plantillas'" + ") </script>");
lblInformationInvoice.Text = "El archivo no coincide con el formato de las plantillas";
return;
}
}
}
}
lblInformationInvoice.Text = "";
File.Delete(path);
}
}
