ContactSolver.smali

.class public Lorg/jbox2d/dynamics/contacts/ContactSolver;
.super Ljava/lang/Object;
.source "ContactSolver.java"


# annotations
.annotation system Ldalvik/annotation/MemberClasses;
    value = {
        Lorg/jbox2d/dynamics/contacts/ContactSolver$ContactSolverDef;
    }
.end annotation


# static fields
.field static final synthetic $assertionsDisabled:Z

.field public static final DEBUG_SOLVER:Z = false

.field public static final INITIAL_NUM_CONSTRAINTS:I = 0x100

.field public static final k_errorTol:F = 0.001f

.field public static final k_maxConditionNumber:F = 100.0f


# instance fields
.field public m_contacts:[Lorg/jbox2d/dynamics/contacts/Contact;

.field public m_count:I

.field public m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

.field public m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

.field public m_step:Lorg/jbox2d/dynamics/TimeStep;

.field public m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

.field public m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

.field private final psolver:Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;

.field private final worldManifold:Lorg/jbox2d/collision/WorldManifold;

.field private final xfA:Lorg/jbox2d/common/Transform;

.field private final xfB:Lorg/jbox2d/common/Transform;


# direct methods
.method static constructor <clinit>()V
    .registers 1

    .prologue
    .line 44
    const-class v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;

    invoke-virtual {v0}, Ljava/lang/Class;->desiredAssertionStatus()Z

    move-result v0

    if-nez v0, :cond_c

    const/4 v0, 0x1

    :goto_9
    sput-boolean v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->$assertionsDisabled:Z

    return-void

    :cond_c
    const/4 v0, 0x0

    goto :goto_9
.end method

.method public constructor <init>()V
    .registers 5

    .prologue
    const/16 v3, 0x100

    .line 67
    invoke-direct {p0}, Ljava/lang/Object;-><init>()V

    .line 217
    new-instance v0, Lorg/jbox2d/common/Transform;

    invoke-direct {v0}, Lorg/jbox2d/common/Transform;-><init>()V

    iput-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    .line 218
    new-instance v0, Lorg/jbox2d/common/Transform;

    invoke-direct {v0}, Lorg/jbox2d/common/Transform;-><init>()V

    iput-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    .line 219
    new-instance v0, Lorg/jbox2d/collision/WorldManifold;

    invoke-direct {v0}, Lorg/jbox2d/collision/WorldManifold;-><init>()V

    iput-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->worldManifold:Lorg/jbox2d/collision/WorldManifold;

    .line 805
    new-instance v0, Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;

    invoke-direct {v0}, Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;-><init>()V

    iput-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->psolver:Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;

    .line 68
    new-array v0, v3, [Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    iput-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    .line 69
    new-array v0, v3, [Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    iput-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    .line 70
    const/4 v0, 0x0

    :goto_2a
    if-ge v0, v3, :cond_41

    .line 71
    iget-object v1, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    new-instance v2, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    invoke-direct {v2}, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;-><init>()V

    aput-object v2, v1, v0

    .line 72
    iget-object v1, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    new-instance v2, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    invoke-direct {v2}, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;-><init>()V

    aput-object v2, v1, v0

    .line 70
    add-int/lit8 v0, v0, 0x1

    goto :goto_2a

    .line 74
    :cond_41
    return-void
.end method


# virtual methods
.method public final init(Lorg/jbox2d/dynamics/contacts/ContactSolver$ContactSolverDef;)V
    .registers 15

    .prologue
    const/4 v12, 0x0

    const/4 v1, 0x0

    .line 78
    iget-object v0, p1, Lorg/jbox2d/dynamics/contacts/ContactSolver$ContactSolverDef;->step:Lorg/jbox2d/dynamics/TimeStep;

    iput-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_step:Lorg/jbox2d/dynamics/TimeStep;

    .line 79
    iget v0, p1, Lorg/jbox2d/dynamics/contacts/ContactSolver$ContactSolverDef;->count:I

    iput v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    .line 81
    iget-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    array-length v0, v0

    iget v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    if-ge v0, v2, :cond_38

    .line 82
    iget-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    .line 83
    array-length v2, v0

    mul-int/lit8 v2, v2, 0x2

    iget v3, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    invoke-static {v2, v3}, Lorg/jbox2d/common/MathUtils;->max(II)I

    move-result v2

    new-array v2, v2, [Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    iput-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    .line 84
    iget-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    array-length v3, v0

    invoke-static {v0, v1, v2, v1, v3}, Ljava/lang/System;->arraycopy(Ljava/lang/Object;ILjava/lang/Object;II)V

    .line 85
    array-length v0, v0

    :goto_27
    iget-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    array-length v2, v2

    if-ge v0, v2, :cond_38

    .line 86
    iget-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    new-instance v3, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    invoke-direct {v3}, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;-><init>()V

    aput-object v3, v2, v0

    .line 85
    add-int/lit8 v0, v0, 0x1

    goto :goto_27

    .line 90
    :cond_38
    iget-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    array-length v0, v0

    iget v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    if-ge v0, v2, :cond_66

    .line 91
    iget-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    .line 92
    array-length v2, v0

    mul-int/lit8 v2, v2, 0x2

    iget v3, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    invoke-static {v2, v3}, Lorg/jbox2d/common/MathUtils;->max(II)I

    move-result v2

    new-array v2, v2, [Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    iput-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    .line 93
    iget-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    array-length v3, v0

    invoke-static {v0, v1, v2, v1, v3}, Ljava/lang/System;->arraycopy(Ljava/lang/Object;ILjava/lang/Object;II)V

    .line 94
    array-length v0, v0

    :goto_55
    iget-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    array-length v2, v2

    if-ge v0, v2, :cond_66

    .line 95
    iget-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    new-instance v3, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    invoke-direct {v3}, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;-><init>()V

    aput-object v3, v2, v0

    .line 94
    add-int/lit8 v0, v0, 0x1

    goto :goto_55

    .line 99
    :cond_66
    iget-object v0, p1, Lorg/jbox2d/dynamics/contacts/ContactSolver$ContactSolverDef;->positions:[Lorg/jbox2d/dynamics/contacts/Position;

    iput-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    .line 100
    iget-object v0, p1, Lorg/jbox2d/dynamics/contacts/ContactSolver$ContactSolverDef;->velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    iput-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    .line 101
    iget-object v0, p1, Lorg/jbox2d/dynamics/contacts/ContactSolver$ContactSolverDef;->contacts:[Lorg/jbox2d/dynamics/contacts/Contact;

    iput-object v0, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_contacts:[Lorg/jbox2d/dynamics/contacts/Contact;

    move v0, v1

    .line 103
    :goto_73
    iget v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    if-ge v0, v2, :cond_174

    .line 105
    iget-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_contacts:[Lorg/jbox2d/dynamics/contacts/Contact;

    aget-object v2, v2, v0

    .line 107
    iget-object v3, v2, Lorg/jbox2d/dynamics/contacts/Contact;->m_fixtureA:Lorg/jbox2d/dynamics/Fixture;

    .line 108
    iget-object v4, v2, Lorg/jbox2d/dynamics/contacts/Contact;->m_fixtureB:Lorg/jbox2d/dynamics/Fixture;

    .line 109
    invoke-virtual {v3}, Lorg/jbox2d/dynamics/Fixture;->getShape()Lorg/jbox2d/collision/shapes/Shape;

    move-result-object v5

    .line 110
    invoke-virtual {v4}, Lorg/jbox2d/dynamics/Fixture;->getShape()Lorg/jbox2d/collision/shapes/Shape;

    move-result-object v6

    .line 111
    iget v5, v5, Lorg/jbox2d/collision/shapes/Shape;->m_radius:F

    .line 112
    iget v6, v6, Lorg/jbox2d/collision/shapes/Shape;->m_radius:F

    .line 113
    invoke-virtual {v3}, Lorg/jbox2d/dynamics/Fixture;->getBody()Lorg/jbox2d/dynamics/Body;

    move-result-object v3

    .line 114
    invoke-virtual {v4}, Lorg/jbox2d/dynamics/Fixture;->getBody()Lorg/jbox2d/dynamics/Body;

    move-result-object v4

    .line 115
    invoke-virtual {v2}, Lorg/jbox2d/dynamics/contacts/Contact;->getManifold()Lorg/jbox2d/collision/Manifold;

    move-result-object v7

    .line 117
    iget v8, v7, Lorg/jbox2d/collision/Manifold;->pointCount:I

    .line 118
    sget-boolean v9, Lorg/jbox2d/dynamics/contacts/ContactSolver;->$assertionsDisabled:Z

    if-nez v9, :cond_a5

    if-gtz v8, :cond_a5

    new-instance v0, Ljava/lang/AssertionError;

    invoke-direct {v0}, Ljava/lang/AssertionError;-><init>()V

    throw v0

    .line 120
    :cond_a5
    iget-object v9, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    aget-object v9, v9, v0

    .line 121
    iget v10, v2, Lorg/jbox2d/dynamics/contacts/Contact;->m_friction:F

    iput v10, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->friction:F

    .line 122
    iget v10, v2, Lorg/jbox2d/dynamics/contacts/Contact;->m_restitution:F

    iput v10, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->restitution:F

    .line 123
    iget v2, v2, Lorg/jbox2d/dynamics/contacts/Contact;->m_tangentSpeed:F

    iput v2, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->tangentSpeed:F

    .line 124
    iget v2, v3, Lorg/jbox2d/dynamics/Body;->m_islandIndex:I

    iput v2, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->indexA:I

    .line 125
    iget v2, v4, Lorg/jbox2d/dynamics/Body;->m_islandIndex:I

    iput v2, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->indexB:I

    .line 126
    iget v2, v3, Lorg/jbox2d/dynamics/Body;->m_invMass:F

    iput v2, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invMassA:F

    .line 127
    iget v2, v4, Lorg/jbox2d/dynamics/Body;->m_invMass:F

    iput v2, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invMassB:F

    .line 128
    iget v2, v3, Lorg/jbox2d/dynamics/Body;->m_invI:F

    iput v2, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invIA:F

    .line 129
    iget v2, v4, Lorg/jbox2d/dynamics/Body;->m_invI:F

    iput v2, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invIB:F

    .line 130
    iput v0, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->contactIndex:I

    .line 131
    iput v8, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->pointCount:I

    .line 132
    iget-object v2, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->K:Lorg/jbox2d/common/Mat22;

    invoke-virtual {v2}, Lorg/jbox2d/common/Mat22;->setZero()V

    .line 133
    iget-object v2, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->normalMass:Lorg/jbox2d/common/Mat22;

    invoke-virtual {v2}, Lorg/jbox2d/common/Mat22;->setZero()V

    .line 135
    iget-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    aget-object v10, v2, v0

    .line 136
    iget v2, v3, Lorg/jbox2d/dynamics/Body;->m_islandIndex:I

    iput v2, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->indexA:I

    .line 137
    iget v2, v4, Lorg/jbox2d/dynamics/Body;->m_islandIndex:I

    iput v2, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->indexB:I

    .line 138
    iget v2, v3, Lorg/jbox2d/dynamics/Body;->m_invMass:F

    iput v2, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invMassA:F

    .line 139
    iget v2, v4, Lorg/jbox2d/dynamics/Body;->m_invMass:F

    iput v2, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invMassB:F

    .line 140
    iget-object v2, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localCenterA:Lorg/jbox2d/common/Vec2;

    iget-object v11, v3, Lorg/jbox2d/dynamics/Body;->m_sweep:Lorg/jbox2d/common/Sweep;

    iget-object v11, v11, Lorg/jbox2d/common/Sweep;->localCenter:Lorg/jbox2d/common/Vec2;

    invoke-virtual {v2, v11}, Lorg/jbox2d/common/Vec2;->set(Lorg/jbox2d/common/Vec2;)Lorg/jbox2d/common/Vec2;

    .line 141
    iget-object v2, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localCenterB:Lorg/jbox2d/common/Vec2;

    iget-object v11, v4, Lorg/jbox2d/dynamics/Body;->m_sweep:Lorg/jbox2d/common/Sweep;

    iget-object v11, v11, Lorg/jbox2d/common/Sweep;->localCenter:Lorg/jbox2d/common/Vec2;

    invoke-virtual {v2, v11}, Lorg/jbox2d/common/Vec2;->set(Lorg/jbox2d/common/Vec2;)Lorg/jbox2d/common/Vec2;

    .line 142
    iget v2, v3, Lorg/jbox2d/dynamics/Body;->m_invI:F

    iput v2, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invIA:F

    .line 143
    iget v2, v4, Lorg/jbox2d/dynamics/Body;->m_invI:F

    iput v2, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invIB:F

    .line 144
    iget-object v2, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localNormal:Lorg/jbox2d/common/Vec2;

    iget-object v3, v7, Lorg/jbox2d/collision/Manifold;->localNormal:Lorg/jbox2d/common/Vec2;

    invoke-virtual {v2, v3}, Lorg/jbox2d/common/Vec2;->set(Lorg/jbox2d/common/Vec2;)Lorg/jbox2d/common/Vec2;

    .line 145
    iget-object v2, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localPoint:Lorg/jbox2d/common/Vec2;

    iget-object v3, v7, Lorg/jbox2d/collision/Manifold;->localPoint:Lorg/jbox2d/common/Vec2;

    invoke-virtual {v2, v3}, Lorg/jbox2d/common/Vec2;->set(Lorg/jbox2d/common/Vec2;)Lorg/jbox2d/common/Vec2;

    .line 146
    iput v8, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->pointCount:I

    .line 147
    iput v5, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->radiusA:F

    .line 148
    iput v6, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->radiusB:F

    .line 149
    iget-object v2, v7, Lorg/jbox2d/collision/Manifold;->type:Lorg/jbox2d/collision/Manifold$ManifoldType;

    iput-object v2, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->type:Lorg/jbox2d/collision/Manifold$ManifoldType;

    move v2, v1

    .line 152
    :goto_122
    if-ge v2, v8, :cond_170

    .line 153
    iget-object v3, v7, Lorg/jbox2d/collision/Manifold;->points:[Lorg/jbox2d/collision/ManifoldPoint;

    aget-object v3, v3, v2

    .line 154
    iget-object v4, v9, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->points:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;

    aget-object v4, v4, v2

    .line 156
    iget-object v5, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_step:Lorg/jbox2d/dynamics/TimeStep;

    iget-boolean v5, v5, Lorg/jbox2d/dynamics/TimeStep;->warmStarting:Z

    if-eqz v5, :cond_16b

    .line 159
    iget-object v5, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_step:Lorg/jbox2d/dynamics/TimeStep;

    iget v5, v5, Lorg/jbox2d/dynamics/TimeStep;->dtRatio:F

    iget v6, v3, Lorg/jbox2d/collision/ManifoldPoint;->normalImpulse:F

    mul-float/2addr v5, v6

    iput v5, v4, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    .line 160
    iget-object v5, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_step:Lorg/jbox2d/dynamics/TimeStep;

    iget v5, v5, Lorg/jbox2d/dynamics/TimeStep;->dtRatio:F

    iget v6, v3, Lorg/jbox2d/collision/ManifoldPoint;->tangentImpulse:F

    mul-float/2addr v5, v6

    iput v5, v4, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->tangentImpulse:F

    .line 166
    :goto_144
    iget-object v5, v4, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    invoke-virtual {v5}, Lorg/jbox2d/common/Vec2;->setZero()V

    .line 167
    iget-object v5, v4, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    invoke-virtual {v5}, Lorg/jbox2d/common/Vec2;->setZero()V

    .line 168
    iput v12, v4, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalMass:F

    .line 169
    iput v12, v4, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->tangentMass:F

    .line 170
    iput v12, v4, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->velocityBias:F

    .line 171
    iget-object v4, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localPoints:[Lorg/jbox2d/common/Vec2;

    aget-object v4, v4, v2

    iget-object v5, v3, Lorg/jbox2d/collision/ManifoldPoint;->localPoint:Lorg/jbox2d/common/Vec2;

    iget v5, v5, Lorg/jbox2d/common/Vec2;->x:F

    iput v5, v4, Lorg/jbox2d/common/Vec2;->x:F

    .line 172
    iget-object v4, v10, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localPoints:[Lorg/jbox2d/common/Vec2;

    aget-object v4, v4, v2

    iget-object v3, v3, Lorg/jbox2d/collision/ManifoldPoint;->localPoint:Lorg/jbox2d/common/Vec2;

    iget v3, v3, Lorg/jbox2d/common/Vec2;->y:F

    iput v3, v4, Lorg/jbox2d/common/Vec2;->y:F

    .line 152
    add-int/lit8 v2, v2, 0x1

    goto :goto_122

    .line 162
    :cond_16b
    iput v12, v4, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    .line 163
    iput v12, v4, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->tangentImpulse:F

    goto :goto_144

    .line 103
    :cond_170
    add-int/lit8 v0, v0, 0x1

    goto/16 :goto_73

    .line 175
    :cond_174
    return-void
.end method

.method public final initializeVelocityConstraints()V
    .registers 26

    .prologue
    .line 224
    const/4 v1, 0x0

    move v7, v1

    :goto_2
    move-object/from16 v0, p0

    iget v1, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    if-ge v7, v1, :cond_2fc

    .line 225
    move-object/from16 v0, p0

    iget-object v1, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    aget-object v8, v1, v7

    .line 226
    move-object/from16 v0, p0

    iget-object v1, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    aget-object v1, v1, v7

    .line 228
    iget v4, v1, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->radiusA:F

    .line 229
    iget v6, v1, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->radiusB:F

    .line 230
    move-object/from16 v0, p0

    iget-object v2, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_contacts:[Lorg/jbox2d/dynamics/contacts/Contact;

    iget v3, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->contactIndex:I

    aget-object v2, v2, v3

    invoke-virtual {v2}, Lorg/jbox2d/dynamics/contacts/Contact;->getManifold()Lorg/jbox2d/collision/Manifold;

    move-result-object v2

    .line 232
    iget v3, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->indexA:I

    .line 233
    iget v5, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->indexB:I

    .line 235
    iget v9, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invMassA:F

    .line 236
    iget v10, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invMassB:F

    .line 237
    iget v11, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invIA:F

    .line 238
    iget v12, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invIB:F

    .line 239
    iget-object v13, v1, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localCenterA:Lorg/jbox2d/common/Vec2;

    .line 240
    iget-object v1, v1, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localCenterB:Lorg/jbox2d/common/Vec2;

    .line 242
    move-object/from16 v0, p0

    iget-object v14, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v14, v14, v3

    iget-object v14, v14, Lorg/jbox2d/dynamics/contacts/Position;->c:Lorg/jbox2d/common/Vec2;

    .line 243
    move-object/from16 v0, p0

    iget-object v15, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v15, v15, v3

    iget v15, v15, Lorg/jbox2d/dynamics/contacts/Position;->a:F

    .line 244
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    move-object/from16 v16, v0

    aget-object v16, v16, v3

    move-object/from16 v0, v16

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/Velocity;->v:Lorg/jbox2d/common/Vec2;

    move-object/from16 v16, v0

    .line 245
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    move-object/from16 v17, v0

    aget-object v3, v17, v3

    iget v0, v3, Lorg/jbox2d/dynamics/contacts/Velocity;->w:F

    move/from16 v17, v0

    .line 247
    move-object/from16 v0, p0

    iget-object v3, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v3, v3, v5

    iget-object v0, v3, Lorg/jbox2d/dynamics/contacts/Position;->c:Lorg/jbox2d/common/Vec2;

    move-object/from16 v18, v0

    .line 248
    move-object/from16 v0, p0

    iget-object v3, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v3, v3, v5

    iget v3, v3, Lorg/jbox2d/dynamics/contacts/Position;->a:F

    .line 249
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    move-object/from16 v19, v0

    aget-object v19, v19, v5

    move-object/from16 v0, v19

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/Velocity;->v:Lorg/jbox2d/common/Vec2;

    move-object/from16 v19, v0

    .line 250
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    move-object/from16 v20, v0

    aget-object v5, v20, v5

    iget v0, v5, Lorg/jbox2d/dynamics/contacts/Velocity;->w:F

    move/from16 v20, v0

    .line 252
    sget-boolean v5, Lorg/jbox2d/dynamics/contacts/ContactSolver;->$assertionsDisabled:Z

    if-nez v5, :cond_98

    iget v5, v2, Lorg/jbox2d/collision/Manifold;->pointCount:I

    if-gtz v5, :cond_98

    new-instance v1, Ljava/lang/AssertionError;

    invoke-direct {v1}, Ljava/lang/AssertionError;-><init>()V

    throw v1

    .line 254
    :cond_98
    move-object/from16 v0, p0

    iget-object v5, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    iget-object v5, v5, Lorg/jbox2d/common/Transform;->q:Lorg/jbox2d/common/Rot;

    .line 255
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    move-object/from16 v21, v0

    move-object/from16 v0, v21

    iget-object v0, v0, Lorg/jbox2d/common/Transform;->q:Lorg/jbox2d/common/Rot;

    move-object/from16 v21, v0

    .line 256
    invoke-virtual {v5, v15}, Lorg/jbox2d/common/Rot;->set(F)Lorg/jbox2d/common/Rot;

    .line 257
    move-object/from16 v0, v21

    invoke-virtual {v0, v3}, Lorg/jbox2d/common/Rot;->set(F)Lorg/jbox2d/common/Rot;

    .line 258
    move-object/from16 v0, p0

    iget-object v3, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    iget-object v3, v3, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    iget v15, v14, Lorg/jbox2d/common/Vec2;->x:F

    iget v0, v5, Lorg/jbox2d/common/Rot;->c:F

    move/from16 v22, v0

    iget v0, v13, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v23, v0

    mul-float v22, v22, v23

    iget v0, v5, Lorg/jbox2d/common/Rot;->s:F

    move/from16 v23, v0

    iget v0, v13, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v24, v0

    mul-float v23, v23, v24

    sub-float v22, v22, v23

    sub-float v15, v15, v22

    iput v15, v3, Lorg/jbox2d/common/Vec2;->x:F

    .line 259
    move-object/from16 v0, p0

    iget-object v3, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    iget-object v3, v3, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    iget v15, v14, Lorg/jbox2d/common/Vec2;->y:F

    iget v0, v5, Lorg/jbox2d/common/Rot;->s:F

    move/from16 v22, v0

    iget v0, v13, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v23, v0

    mul-float v22, v22, v23

    iget v5, v5, Lorg/jbox2d/common/Rot;->c:F

    iget v13, v13, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v5, v13

    add-float v5, v5, v22

    sub-float v5, v15, v5

    iput v5, v3, Lorg/jbox2d/common/Vec2;->y:F

    .line 260
    move-object/from16 v0, p0

    iget-object v3, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    iget-object v3, v3, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    move-object/from16 v0, v18

    iget v5, v0, Lorg/jbox2d/common/Vec2;->x:F

    move-object/from16 v0, v21

    iget v13, v0, Lorg/jbox2d/common/Rot;->c:F

    iget v15, v1, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v13, v15

    move-object/from16 v0, v21

    iget v15, v0, Lorg/jbox2d/common/Rot;->s:F

    iget v0, v1, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v22, v0

    mul-float v15, v15, v22

    sub-float/2addr v13, v15

    sub-float/2addr v5, v13

    iput v5, v3, Lorg/jbox2d/common/Vec2;->x:F

    .line 261
    move-object/from16 v0, p0

    iget-object v3, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    iget-object v3, v3, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    move-object/from16 v0, v18

    iget v5, v0, Lorg/jbox2d/common/Vec2;->y:F

    move-object/from16 v0, v21

    iget v13, v0, Lorg/jbox2d/common/Rot;->s:F

    iget v15, v1, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v13, v15

    move-object/from16 v0, v21

    iget v15, v0, Lorg/jbox2d/common/Rot;->c:F

    iget v1, v1, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v1, v15

    add-float/2addr v1, v13

    sub-float v1, v5, v1

    iput v1, v3, Lorg/jbox2d/common/Vec2;->y:F

    .line 263
    move-object/from16 v0, p0

    iget-object v1, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->worldManifold:Lorg/jbox2d/collision/WorldManifold;

    move-object/from16 v0, p0

    iget-object v3, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    move-object/from16 v0, p0

    iget-object v5, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    invoke-virtual/range {v1 .. v6}, Lorg/jbox2d/collision/WorldManifold;->initialize(Lorg/jbox2d/collision/Manifold;Lorg/jbox2d/common/Transform;FLorg/jbox2d/common/Transform;F)V

    .line 265
    iget-object v3, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->normal:Lorg/jbox2d/common/Vec2;

    .line 266
    move-object/from16 v0, p0

    iget-object v1, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->worldManifold:Lorg/jbox2d/collision/WorldManifold;

    iget-object v1, v1, Lorg/jbox2d/collision/WorldManifold;->normal:Lorg/jbox2d/common/Vec2;

    iget v1, v1, Lorg/jbox2d/common/Vec2;->x:F

    iput v1, v3, Lorg/jbox2d/common/Vec2;->x:F

    .line 267
    move-object/from16 v0, p0

    iget-object v1, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->worldManifold:Lorg/jbox2d/collision/WorldManifold;

    iget-object v1, v1, Lorg/jbox2d/collision/WorldManifold;->normal:Lorg/jbox2d/common/Vec2;

    iget v1, v1, Lorg/jbox2d/common/Vec2;->y:F

    iput v1, v3, Lorg/jbox2d/common/Vec2;->y:F

    .line 269
    iget v4, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->pointCount:I

    .line 270
    const/4 v1, 0x0

    move v2, v1

    :goto_156
    if-ge v2, v4, :cond_25e

    .line 271
    iget-object v1, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->points:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;

    aget-object v5, v1, v2

    .line 272
    move-object/from16 v0, p0

    iget-object v1, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->worldManifold:Lorg/jbox2d/collision/WorldManifold;

    iget-object v1, v1, Lorg/jbox2d/collision/WorldManifold;->points:[Lorg/jbox2d/common/Vec2;

    aget-object v1, v1, v2

    .line 273
    iget-object v6, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    .line 274
    iget-object v13, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    .line 275
    iget v15, v1, Lorg/jbox2d/common/Vec2;->x:F

    iget v0, v14, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v21, v0

    sub-float v15, v15, v21

    iput v15, v6, Lorg/jbox2d/common/Vec2;->x:F

    .line 276
    iget v15, v1, Lorg/jbox2d/common/Vec2;->y:F

    iget v0, v14, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v21, v0

    sub-float v15, v15, v21

    iput v15, v6, Lorg/jbox2d/common/Vec2;->y:F

    .line 277
    iget v15, v1, Lorg/jbox2d/common/Vec2;->x:F

    move-object/from16 v0, v18

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v21, v0

    sub-float v15, v15, v21

    iput v15, v13, Lorg/jbox2d/common/Vec2;->x:F

    .line 278
    iget v1, v1, Lorg/jbox2d/common/Vec2;->y:F

    move-object/from16 v0, v18

    iget v15, v0, Lorg/jbox2d/common/Vec2;->y:F

    sub-float/2addr v1, v15

    iput v1, v13, Lorg/jbox2d/common/Vec2;->y:F

    .line 280
    iget v1, v6, Lorg/jbox2d/common/Vec2;->x:F

    iget v15, v3, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v1, v15

    iget v15, v6, Lorg/jbox2d/common/Vec2;->y:F

    iget v0, v3, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v21, v0

    mul-float v15, v15, v21

    sub-float/2addr v1, v15

    .line 281
    iget v15, v13, Lorg/jbox2d/common/Vec2;->x:F

    iget v0, v3, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v21, v0

    mul-float v15, v15, v21

    iget v0, v13, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v21, v0

    iget v0, v3, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v22, v0

    mul-float v21, v21, v22

    sub-float v15, v15, v21

    .line 283
    add-float v21, v9, v10

    mul-float v22, v11, v1

    mul-float v1, v1, v22

    add-float v1, v1, v21

    mul-float v21, v12, v15

    mul-float v15, v15, v21

    add-float/2addr v1, v15

    .line 285
    const/4 v15, 0x0

    cmpl-float v15, v1, v15

    if-lez v15, :cond_259

    const/high16 v15, 0x3f800000    # 1.0f

    div-float v1, v15, v1

    :goto_1c9
    iput v1, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalMass:F

    .line 287
    const/high16 v1, 0x3f800000    # 1.0f

    iget v15, v3, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v1, v15

    .line 288
    const/high16 v15, -0x40800000    # -1.0f

    iget v0, v3, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v21, v0

    mul-float v15, v15, v21

    .line 290
    iget v0, v6, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v21, v0

    mul-float v21, v21, v15

    iget v0, v6, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v22, v0

    mul-float v22, v22, v1

    sub-float v21, v21, v22

    .line 291
    iget v0, v13, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v22, v0

    mul-float v15, v15, v22

    iget v0, v13, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v22, v0

    mul-float v1, v1, v22

    sub-float v1, v15, v1

    .line 293
    add-float v15, v9, v10

    mul-float v22, v11, v21

    mul-float v21, v21, v22

    add-float v15, v15, v21

    mul-float v21, v12, v1

    mul-float v1, v1, v21

    add-float/2addr v1, v15

    .line 295
    const/4 v15, 0x0

    cmpl-float v15, v1, v15

    if-lez v15, :cond_25c

    const/high16 v15, 0x3f800000    # 1.0f

    div-float v1, v15, v1

    :goto_20a
    iput v1, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->tangentMass:F

    .line 298
    const/4 v1, 0x0

    iput v1, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->velocityBias:F

    .line 299
    move-object/from16 v0, v19

    iget v1, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v0, v20

    neg-float v15, v0

    iget v0, v13, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v21, v0

    mul-float v15, v15, v21

    add-float/2addr v1, v15

    move-object/from16 v0, v16

    iget v15, v0, Lorg/jbox2d/common/Vec2;->x:F

    sub-float/2addr v1, v15

    move/from16 v0, v17

    neg-float v15, v0

    iget v0, v6, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v21, v0

    mul-float v15, v15, v21

    sub-float/2addr v1, v15

    .line 300
    move-object/from16 v0, v19

    iget v15, v0, Lorg/jbox2d/common/Vec2;->y:F

    iget v13, v13, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v13, v13, v20

    add-float/2addr v13, v15

    move-object/from16 v0, v16

    iget v15, v0, Lorg/jbox2d/common/Vec2;->y:F

    sub-float/2addr v13, v15

    iget v6, v6, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v6, v6, v17

    sub-float v6, v13, v6

    .line 301
    iget v13, v3, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v1, v13

    iget v13, v3, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v6, v13

    add-float/2addr v1, v6

    .line 302
    sget v6, Lorg/jbox2d/common/Settings;->velocityThreshold:F

    neg-float v6, v6

    cmpg-float v6, v1, v6

    if-gez v6, :cond_254

    .line 303
    iget v6, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->restitution:F

    neg-float v6, v6

    mul-float/2addr v1, v6

    iput v1, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->velocityBias:F

    .line 270
    :cond_254
    add-int/lit8 v1, v2, 0x1

    move v2, v1

    goto/16 :goto_156

    .line 285
    :cond_259
    const/4 v1, 0x0

    goto/16 :goto_1c9

    .line 295
    :cond_25c
    const/4 v1, 0x0

    goto :goto_20a

    .line 308
    :cond_25e
    iget v1, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->pointCount:I

    const/4 v2, 0x2

    if-ne v1, v2, :cond_2f3

    .line 309
    iget-object v1, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->points:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;

    const/4 v2, 0x0

    aget-object v1, v1, v2

    .line 310
    iget-object v2, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->points:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;

    const/4 v4, 0x1

    aget-object v2, v2, v4

    .line 311
    iget-object v4, v1, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    iget v4, v4, Lorg/jbox2d/common/Vec2;->x:F

    iget v5, v3, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v4, v5

    iget-object v5, v1, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    iget v5, v5, Lorg/jbox2d/common/Vec2;->y:F

    iget v6, v3, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v5, v6

    sub-float/2addr v4, v5

    .line 312
    iget-object v5, v1, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    iget v5, v5, Lorg/jbox2d/common/Vec2;->x:F

    iget v6, v3, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v5, v6

    iget-object v1, v1, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    iget v1, v1, Lorg/jbox2d/common/Vec2;->y:F

    iget v6, v3, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v1, v6

    sub-float v1, v5, v1

    .line 313
    iget-object v5, v2, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    iget v5, v5, Lorg/jbox2d/common/Vec2;->x:F

    iget v6, v3, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v5, v6

    iget-object v6, v2, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    iget v6, v6, Lorg/jbox2d/common/Vec2;->y:F

    iget v13, v3, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v6, v13

    sub-float/2addr v5, v6

    .line 314
    iget-object v6, v2, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    iget v6, v6, Lorg/jbox2d/common/Vec2;->x:F

    iget v13, v3, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v6, v13

    iget-object v2, v2, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    iget v2, v2, Lorg/jbox2d/common/Vec2;->y:F

    iget v3, v3, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v2, v3

    sub-float v2, v6, v2

    .line 316
    add-float v3, v9, v10

    mul-float v6, v11, v4

    mul-float/2addr v6, v4

    add-float/2addr v3, v6

    mul-float v6, v12, v1

    mul-float/2addr v6, v1

    add-float/2addr v3, v6

    .line 317
    add-float v6, v9, v10

    mul-float v13, v11, v5

    mul-float/2addr v13, v5

    add-float/2addr v6, v13

    mul-float v13, v12, v2

    mul-float/2addr v13, v2

    add-float/2addr v6, v13

    .line 318
    add-float/2addr v9, v10

    mul-float/2addr v4, v11

    mul-float/2addr v4, v5

    add-float/2addr v4, v9

    mul-float/2addr v1, v12

    mul-float/2addr v1, v2

    add-float/2addr v1, v4

    .line 319
    mul-float v2, v3, v3

    const/high16 v4, 0x42c80000    # 100.0f

    mul-float v5, v3, v6

    mul-float v9, v1, v1

    sub-float/2addr v5, v9

    mul-float/2addr v4, v5

    cmpg-float v2, v2, v4

    if-gez v2, :cond_2f8

    .line 321
    iget-object v2, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->K:Lorg/jbox2d/common/Mat22;

    iget-object v2, v2, Lorg/jbox2d/common/Mat22;->ex:Lorg/jbox2d/common/Vec2;

    iput v3, v2, Lorg/jbox2d/common/Vec2;->x:F

    .line 322
    iget-object v2, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->K:Lorg/jbox2d/common/Mat22;

    iget-object v2, v2, Lorg/jbox2d/common/Mat22;->ex:Lorg/jbox2d/common/Vec2;

    iput v1, v2, Lorg/jbox2d/common/Vec2;->y:F

    .line 323
    iget-object v2, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->K:Lorg/jbox2d/common/Mat22;

    iget-object v2, v2, Lorg/jbox2d/common/Mat22;->ey:Lorg/jbox2d/common/Vec2;

    iput v1, v2, Lorg/jbox2d/common/Vec2;->x:F

    .line 324
    iget-object v1, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->K:Lorg/jbox2d/common/Mat22;

    iget-object v1, v1, Lorg/jbox2d/common/Mat22;->ey:Lorg/jbox2d/common/Vec2;

    iput v6, v1, Lorg/jbox2d/common/Vec2;->y:F

    .line 325
    iget-object v1, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->K:Lorg/jbox2d/common/Mat22;

    iget-object v2, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->normalMass:Lorg/jbox2d/common/Mat22;

    invoke-virtual {v1, v2}, Lorg/jbox2d/common/Mat22;->invertToOut(Lorg/jbox2d/common/Mat22;)V

    .line 224
    :cond_2f3
    :goto_2f3
    add-int/lit8 v1, v7, 0x1

    move v7, v1

    goto/16 :goto_2

    .line 329
    :cond_2f8
    const/4 v1, 0x1

    iput v1, v8, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->pointCount:I

    goto :goto_2f3

    .line 333
    :cond_2fc
    return-void
.end method

.method public final solvePositionConstraints()Z
    .registers 32

    .prologue
    .line 811
    const/4 v3, 0x0

    .line 813
    const/4 v2, 0x0

    :goto_2
    move-object/from16 v0, p0

    iget v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    if-ge v2, v4, :cond_232

    .line 814
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    aget-object v8, v4, v2

    .line 816
    iget v9, v8, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->indexA:I

    .line 817
    iget v10, v8, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->indexB:I

    .line 819
    iget v11, v8, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invMassA:F

    .line 820
    iget v12, v8, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invIA:F

    .line 821
    iget-object v4, v8, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localCenterA:Lorg/jbox2d/common/Vec2;

    .line 822
    iget v13, v4, Lorg/jbox2d/common/Vec2;->x:F

    .line 823
    iget v14, v4, Lorg/jbox2d/common/Vec2;->y:F

    .line 824
    iget v15, v8, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invMassB:F

    .line 825
    iget v0, v8, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invIB:F

    move/from16 v16, v0

    .line 826
    iget-object v4, v8, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localCenterB:Lorg/jbox2d/common/Vec2;

    .line 827
    iget v0, v4, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v17, v0

    .line 828
    iget v0, v4, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v18, v0

    .line 829
    iget v0, v8, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->pointCount:I

    move/from16 v19, v0

    .line 831
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v4, v4, v9

    iget-object v0, v4, Lorg/jbox2d/dynamics/contacts/Position;->c:Lorg/jbox2d/common/Vec2;

    move-object/from16 v20, v0

    .line 832
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v4, v4, v9

    iget v6, v4, Lorg/jbox2d/dynamics/contacts/Position;->a:F

    .line 833
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v4, v4, v10

    iget-object v0, v4, Lorg/jbox2d/dynamics/contacts/Position;->c:Lorg/jbox2d/common/Vec2;

    move-object/from16 v21, v0

    .line 834
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v4, v4, v10

    iget v5, v4, Lorg/jbox2d/dynamics/contacts/Position;->a:F

    .line 837
    const/4 v4, 0x0

    :goto_55
    move/from16 v0, v19

    if-ge v4, v0, :cond_21e

    .line 838
    move-object/from16 v0, p0

    iget-object v7, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    iget-object v7, v7, Lorg/jbox2d/common/Transform;->q:Lorg/jbox2d/common/Rot;

    .line 839
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    move-object/from16 v22, v0

    move-object/from16 v0, v22

    iget-object v0, v0, Lorg/jbox2d/common/Transform;->q:Lorg/jbox2d/common/Rot;

    move-object/from16 v22, v0

    .line 840
    invoke-virtual {v7, v6}, Lorg/jbox2d/common/Rot;->set(F)Lorg/jbox2d/common/Rot;

    .line 841
    move-object/from16 v0, v22

    invoke-virtual {v0, v5}, Lorg/jbox2d/common/Rot;->set(F)Lorg/jbox2d/common/Rot;

    .line 842
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    move-object/from16 v23, v0

    move-object/from16 v0, v23

    iget-object v0, v0, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    move-object/from16 v23, v0

    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v24, v0

    iget v0, v7, Lorg/jbox2d/common/Rot;->c:F

    move/from16 v25, v0

    mul-float v25, v25, v13

    sub-float v24, v24, v25

    iget v0, v7, Lorg/jbox2d/common/Rot;->s:F

    move/from16 v25, v0

    mul-float v25, v25, v14

    add-float v24, v24, v25

    move/from16 v0, v24

    move-object/from16 v1, v23

    iput v0, v1, Lorg/jbox2d/common/Vec2;->x:F

    .line 843
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    move-object/from16 v23, v0

    move-object/from16 v0, v23

    iget-object v0, v0, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    move-object/from16 v23, v0

    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v24, v0

    iget v0, v7, Lorg/jbox2d/common/Rot;->s:F

    move/from16 v25, v0

    mul-float v25, v25, v13

    sub-float v24, v24, v25

    iget v7, v7, Lorg/jbox2d/common/Rot;->c:F

    mul-float/2addr v7, v14

    sub-float v7, v24, v7

    move-object/from16 v0, v23

    iput v7, v0, Lorg/jbox2d/common/Vec2;->y:F

    .line 844
    move-object/from16 v0, p0

    iget-object v7, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    iget-object v7, v7, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v23, v0

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Rot;->c:F

    move/from16 v24, v0

    mul-float v24, v24, v17

    sub-float v23, v23, v24

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Rot;->s:F

    move/from16 v24, v0

    mul-float v24, v24, v18

    add-float v23, v23, v24

    move/from16 v0, v23

    iput v0, v7, Lorg/jbox2d/common/Vec2;->x:F

    .line 845
    move-object/from16 v0, p0

    iget-object v7, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    iget-object v7, v7, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v23, v0

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Rot;->s:F

    move/from16 v24, v0

    mul-float v24, v24, v17

    sub-float v23, v23, v24

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Rot;->c:F

    move/from16 v22, v0

    mul-float v22, v22, v18

    sub-float v22, v23, v22

    move/from16 v0, v22

    iput v0, v7, Lorg/jbox2d/common/Vec2;->y:F

    .line 847
    move-object/from16 v0, p0

    iget-object v7, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->psolver:Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;

    .line 848
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    move-object/from16 v22, v0

    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    move-object/from16 v23, v0

    move-object/from16 v0, v22

    move-object/from16 v1, v23

    invoke-virtual {v7, v8, v0, v1, v4}, Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;->initialize(Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;Lorg/jbox2d/common/Transform;Lorg/jbox2d/common/Transform;I)V

    .line 849
    iget-object v0, v7, Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;->normal:Lorg/jbox2d/common/Vec2;

    move-object/from16 v22, v0

    .line 850
    iget-object v0, v7, Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;->point:Lorg/jbox2d/common/Vec2;

    move-object/from16 v23, v0

    .line 851
    iget v0, v7, Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;->separation:F

    move/from16 v24, v0

    .line 853
    move-object/from16 v0, v23

    iget v7, v0, Lorg/jbox2d/common/Vec2;->x:F

    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v25, v0

    sub-float v25, v7, v25

    .line 854
    move-object/from16 v0, v23

    iget v7, v0, Lorg/jbox2d/common/Vec2;->y:F

    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v26, v0

    sub-float v26, v7, v26

    .line 855
    move-object/from16 v0, v23

    iget v7, v0, Lorg/jbox2d/common/Vec2;->x:F

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v27, v0

    sub-float v27, v7, v27

    .line 856
    move-object/from16 v0, v23

    iget v7, v0, Lorg/jbox2d/common/Vec2;->y:F

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v23, v0

    sub-float v23, v7, v23

    .line 859
    move/from16 v0, v24

    invoke-static {v3, v0}, Lorg/jbox2d/common/MathUtils;->min(FF)F

    move-result v7

    .line 862
    sget v3, Lorg/jbox2d/common/Settings;->baumgarte:F

    sget v28, Lorg/jbox2d/common/Settings;->linearSlop:F

    add-float v24, v24, v28

    mul-float v3, v3, v24

    sget v24, Lorg/jbox2d/common/Settings;->maxLinearCorrection:F

    move/from16 v0, v24

    neg-float v0, v0

    move/from16 v24, v0

    const/16 v28, 0x0

    move/from16 v0, v24

    move/from16 v1, v28

    invoke-static {v3, v0, v1}, Lorg/jbox2d/common/MathUtils;->clamp(FFF)F

    move-result v3

    .line 867
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v24, v0

    mul-float v24, v24, v25

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v28, v0

    mul-float v28, v28, v26

    sub-float v24, v24, v28

    .line 868
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v28, v0

    mul-float v28, v28, v27

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v29, v0

    mul-float v29, v29, v23

    sub-float v28, v28, v29

    .line 869
    add-float v29, v11, v15

    mul-float v30, v12, v24

    mul-float v24, v24, v30

    add-float v24, v24, v29

    mul-float v29, v16, v28

    mul-float v28, v28, v29

    add-float v24, v24, v28

    .line 872
    const/16 v28, 0x0

    cmpl-float v28, v24, v28

    if-lez v28, :cond_21c

    neg-float v3, v3

    div-float v3, v3, v24

    .line 874
    :goto_1b3
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v24, v0

    mul-float v24, v24, v3

    .line 875
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v22, v0

    mul-float v3, v3, v22

    .line 877
    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v22, v0

    mul-float v28, v24, v11

    sub-float v22, v22, v28

    move/from16 v0, v22

    move-object/from16 v1, v20

    iput v0, v1, Lorg/jbox2d/common/Vec2;->x:F

    .line 878
    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v22, v0

    mul-float v28, v3, v11

    sub-float v22, v22, v28

    move/from16 v0, v22

    move-object/from16 v1, v20

    iput v0, v1, Lorg/jbox2d/common/Vec2;->y:F

    .line 879
    mul-float v22, v25, v3

    mul-float v25, v26, v24

    sub-float v22, v22, v25

    mul-float v22, v22, v12

    sub-float v6, v6, v22

    .line 881
    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v22, v0

    mul-float v25, v24, v15

    add-float v22, v22, v25

    move/from16 v0, v22

    move-object/from16 v1, v21

    iput v0, v1, Lorg/jbox2d/common/Vec2;->x:F

    .line 882
    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v22, v0

    mul-float v25, v3, v15

    add-float v22, v22, v25

    move/from16 v0, v22

    move-object/from16 v1, v21

    iput v0, v1, Lorg/jbox2d/common/Vec2;->y:F

    .line 883
    mul-float v3, v3, v27

    mul-float v22, v23, v24

    sub-float v3, v3, v22

    mul-float v3, v3, v16

    add-float/2addr v5, v3

    .line 837
    add-int/lit8 v3, v4, 0x1

    move v4, v3

    move v3, v7

    goto/16 :goto_55

    .line 872
    :cond_21c
    const/4 v3, 0x0

    goto :goto_1b3

    .line 887
    :cond_21e
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v4, v4, v9

    iput v6, v4, Lorg/jbox2d/dynamics/contacts/Position;->a:F

    .line 890
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v4, v4, v10

    iput v5, v4, Lorg/jbox2d/dynamics/contacts/Position;->a:F

    .line 813
    add-int/lit8 v2, v2, 0x1

    goto/16 :goto_2

    .line 895
    :cond_232
    const/high16 v2, -0x3fc00000    # -3.0f

    sget v4, Lorg/jbox2d/common/Settings;->linearSlop:F

    mul-float/2addr v2, v4

    cmpl-float v2, v3, v2

    if-ltz v2, :cond_23d

    const/4 v2, 0x1

    :goto_23c
    return v2

    :cond_23d
    const/4 v2, 0x0

    goto :goto_23c
.end method

.method public solveTOIPositionConstraints(II)Z
    .registers 34

    .prologue
    .line 900
    const/4 v3, 0x0

    .line 902
    const/4 v2, 0x0

    :goto_2
    move-object/from16 v0, p0

    iget v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    if-ge v2, v4, :cond_248

    .line 903
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positionConstraints:[Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;

    aget-object v12, v4, v2

    .line 905
    iget v13, v12, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->indexA:I

    .line 906
    iget v14, v12, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->indexB:I

    .line 907
    iget-object v4, v12, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localCenterA:Lorg/jbox2d/common/Vec2;

    .line 908
    iget-object v5, v12, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->localCenterB:Lorg/jbox2d/common/Vec2;

    .line 909
    iget v15, v4, Lorg/jbox2d/common/Vec2;->x:F

    .line 910
    iget v0, v4, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v16, v0

    .line 911
    iget v0, v5, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v17, v0

    .line 912
    iget v0, v5, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v18, v0

    .line 913
    iget v0, v12, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->pointCount:I

    move/from16 v19, v0

    .line 915
    const/4 v5, 0x0

    .line 916
    const/4 v4, 0x0

    .line 917
    move/from16 v0, p1

    if-eq v13, v0, :cond_32

    move/from16 v0, p2

    if-ne v13, v0, :cond_255

    .line 918
    :cond_32
    iget v5, v12, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invMassA:F

    .line 919
    iget v4, v12, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invIA:F

    move v10, v4

    move v11, v5

    .line 922
    :goto_38
    const/4 v5, 0x0

    .line 923
    const/4 v4, 0x0

    .line 924
    move/from16 v0, p1

    if-eq v14, v0, :cond_42

    move/from16 v0, p2

    if-ne v14, v0, :cond_46

    .line 925
    :cond_42
    iget v5, v12, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invMassB:F

    .line 926
    iget v4, v12, Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;->invIB:F

    .line 929
    :cond_46
    move-object/from16 v0, p0

    iget-object v6, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v6, v6, v13

    iget-object v0, v6, Lorg/jbox2d/dynamics/contacts/Position;->c:Lorg/jbox2d/common/Vec2;

    move-object/from16 v20, v0

    .line 930
    move-object/from16 v0, p0

    iget-object v6, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v6, v6, v13

    iget v8, v6, Lorg/jbox2d/dynamics/contacts/Position;->a:F

    .line 932
    move-object/from16 v0, p0

    iget-object v6, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v6, v6, v14

    iget-object v0, v6, Lorg/jbox2d/dynamics/contacts/Position;->c:Lorg/jbox2d/common/Vec2;

    move-object/from16 v21, v0

    .line 933
    move-object/from16 v0, p0

    iget-object v6, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v6, v6, v14

    iget v7, v6, Lorg/jbox2d/dynamics/contacts/Position;->a:F

    .line 936
    const/4 v6, 0x0

    :goto_6b
    move/from16 v0, v19

    if-ge v6, v0, :cond_234

    .line 937
    move-object/from16 v0, p0

    iget-object v9, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    iget-object v9, v9, Lorg/jbox2d/common/Transform;->q:Lorg/jbox2d/common/Rot;

    .line 938
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    move-object/from16 v22, v0

    move-object/from16 v0, v22

    iget-object v0, v0, Lorg/jbox2d/common/Transform;->q:Lorg/jbox2d/common/Rot;

    move-object/from16 v22, v0

    .line 939
    invoke-virtual {v9, v8}, Lorg/jbox2d/common/Rot;->set(F)Lorg/jbox2d/common/Rot;

    .line 940
    move-object/from16 v0, v22

    invoke-virtual {v0, v7}, Lorg/jbox2d/common/Rot;->set(F)Lorg/jbox2d/common/Rot;

    .line 941
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    move-object/from16 v23, v0

    move-object/from16 v0, v23

    iget-object v0, v0, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    move-object/from16 v23, v0

    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v24, v0

    iget v0, v9, Lorg/jbox2d/common/Rot;->c:F

    move/from16 v25, v0

    mul-float v25, v25, v15

    sub-float v24, v24, v25

    iget v0, v9, Lorg/jbox2d/common/Rot;->s:F

    move/from16 v25, v0

    mul-float v25, v25, v16

    add-float v24, v24, v25

    move/from16 v0, v24

    move-object/from16 v1, v23

    iput v0, v1, Lorg/jbox2d/common/Vec2;->x:F

    .line 942
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    move-object/from16 v23, v0

    move-object/from16 v0, v23

    iget-object v0, v0, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    move-object/from16 v23, v0

    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v24, v0

    iget v0, v9, Lorg/jbox2d/common/Rot;->s:F

    move/from16 v25, v0

    mul-float v25, v25, v15

    sub-float v24, v24, v25

    iget v9, v9, Lorg/jbox2d/common/Rot;->c:F

    mul-float v9, v9, v16

    sub-float v9, v24, v9

    move-object/from16 v0, v23

    iput v9, v0, Lorg/jbox2d/common/Vec2;->y:F

    .line 943
    move-object/from16 v0, p0

    iget-object v9, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    iget-object v9, v9, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v23, v0

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Rot;->c:F

    move/from16 v24, v0

    mul-float v24, v24, v17

    sub-float v23, v23, v24

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Rot;->s:F

    move/from16 v24, v0

    mul-float v24, v24, v18

    add-float v23, v23, v24

    move/from16 v0, v23

    iput v0, v9, Lorg/jbox2d/common/Vec2;->x:F

    .line 944
    move-object/from16 v0, p0

    iget-object v9, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    iget-object v9, v9, Lorg/jbox2d/common/Transform;->p:Lorg/jbox2d/common/Vec2;

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v23, v0

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Rot;->s:F

    move/from16 v24, v0

    mul-float v24, v24, v17

    sub-float v23, v23, v24

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Rot;->c:F

    move/from16 v22, v0

    mul-float v22, v22, v18

    sub-float v22, v23, v22

    move/from16 v0, v22

    iput v0, v9, Lorg/jbox2d/common/Vec2;->y:F

    .line 946
    move-object/from16 v0, p0

    iget-object v9, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->psolver:Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;

    .line 947
    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfA:Lorg/jbox2d/common/Transform;

    move-object/from16 v22, v0

    move-object/from16 v0, p0

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->xfB:Lorg/jbox2d/common/Transform;

    move-object/from16 v23, v0

    move-object/from16 v0, v22

    move-object/from16 v1, v23

    invoke-virtual {v9, v12, v0, v1, v6}, Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;->initialize(Lorg/jbox2d/dynamics/contacts/ContactPositionConstraint;Lorg/jbox2d/common/Transform;Lorg/jbox2d/common/Transform;I)V

    .line 948
    iget-object v0, v9, Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;->normal:Lorg/jbox2d/common/Vec2;

    move-object/from16 v22, v0

    .line 950
    iget-object v0, v9, Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;->point:Lorg/jbox2d/common/Vec2;

    move-object/from16 v23, v0

    .line 951
    iget v0, v9, Lorg/jbox2d/dynamics/contacts/PositionSolverManifold;->separation:F

    move/from16 v24, v0

    .line 953
    move-object/from16 v0, v23

    iget v9, v0, Lorg/jbox2d/common/Vec2;->x:F

    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v25, v0

    sub-float v25, v9, v25

    .line 954
    move-object/from16 v0, v23

    iget v9, v0, Lorg/jbox2d/common/Vec2;->y:F

    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v26, v0

    sub-float v26, v9, v26

    .line 955
    move-object/from16 v0, v23

    iget v9, v0, Lorg/jbox2d/common/Vec2;->x:F

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v27, v0

    sub-float v27, v9, v27

    .line 956
    move-object/from16 v0, v23

    iget v9, v0, Lorg/jbox2d/common/Vec2;->y:F

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v23, v0

    sub-float v23, v9, v23

    .line 959
    move/from16 v0, v24

    invoke-static {v3, v0}, Lorg/jbox2d/common/MathUtils;->min(FF)F

    move-result v9

    .line 962
    sget v3, Lorg/jbox2d/common/Settings;->toiBaugarte:F

    sget v28, Lorg/jbox2d/common/Settings;->linearSlop:F

    add-float v24, v24, v28

    mul-float v3, v3, v24

    sget v24, Lorg/jbox2d/common/Settings;->maxLinearCorrection:F

    move/from16 v0, v24

    neg-float v0, v0

    move/from16 v24, v0

    const/16 v28, 0x0

    move/from16 v0, v24

    move/from16 v1, v28

    invoke-static {v3, v0, v1}, Lorg/jbox2d/common/MathUtils;->clamp(FFF)F

    move-result v3

    .line 967
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v24, v0

    mul-float v24, v24, v25

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v28, v0

    mul-float v28, v28, v26

    sub-float v24, v24, v28

    .line 968
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v28, v0

    mul-float v28, v28, v27

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v29, v0

    mul-float v29, v29, v23

    sub-float v28, v28, v29

    .line 969
    add-float v29, v11, v5

    mul-float v30, v10, v24

    mul-float v24, v24, v30

    add-float v24, v24, v29

    mul-float v29, v4, v28

    mul-float v28, v28, v29

    add-float v24, v24, v28

    .line 972
    const/16 v28, 0x0

    cmpl-float v28, v24, v28

    if-lez v28, :cond_232

    neg-float v3, v3

    div-float v3, v3, v24

    .line 974
    :goto_1ca
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v24, v0

    mul-float v24, v24, v3

    .line 975
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v22, v0

    mul-float v3, v3, v22

    .line 977
    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v22, v0

    mul-float v28, v24, v11

    sub-float v22, v22, v28

    move/from16 v0, v22

    move-object/from16 v1, v20

    iput v0, v1, Lorg/jbox2d/common/Vec2;->x:F

    .line 978
    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v22, v0

    mul-float v28, v3, v11

    sub-float v22, v22, v28

    move/from16 v0, v22

    move-object/from16 v1, v20

    iput v0, v1, Lorg/jbox2d/common/Vec2;->y:F

    .line 979
    mul-float v22, v25, v3

    mul-float v25, v26, v24

    sub-float v22, v22, v25

    mul-float v22, v22, v10

    sub-float v8, v8, v22

    .line 981
    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v22, v0

    mul-float v25, v24, v5

    add-float v22, v22, v25

    move/from16 v0, v22

    move-object/from16 v1, v21

    iput v0, v1, Lorg/jbox2d/common/Vec2;->x:F

    .line 982
    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v22, v0

    mul-float v25, v3, v5

    add-float v22, v22, v25

    move/from16 v0, v22

    move-object/from16 v1, v21

    iput v0, v1, Lorg/jbox2d/common/Vec2;->y:F

    .line 983
    mul-float v3, v3, v27

    mul-float v22, v23, v24

    sub-float v3, v3, v22

    mul-float/2addr v3, v4

    add-float/2addr v7, v3

    .line 936
    add-int/lit8 v3, v6, 0x1

    move v6, v3

    move v3, v9

    goto/16 :goto_6b

    .line 972
    :cond_232
    const/4 v3, 0x0

    goto :goto_1ca

    .line 987
    :cond_234
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v4, v4, v13

    iput v8, v4, Lorg/jbox2d/dynamics/contacts/Position;->a:F

    .line 990
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_positions:[Lorg/jbox2d/dynamics/contacts/Position;

    aget-object v4, v4, v14

    iput v7, v4, Lorg/jbox2d/dynamics/contacts/Position;->a:F

    .line 902
    add-int/lit8 v2, v2, 0x1

    goto/16 :goto_2

    .line 995
    :cond_248
    const/high16 v2, -0x40400000    # -1.5f

    sget v4, Lorg/jbox2d/common/Settings;->linearSlop:F

    mul-float/2addr v2, v4

    cmpl-float v2, v3, v2

    if-ltz v2, :cond_253

    const/4 v2, 0x1

    :goto_252
    return v2

    :cond_253
    const/4 v2, 0x0

    goto :goto_252

    :cond_255
    move v10, v4

    move v11, v5

    goto/16 :goto_38
.end method

.method public final solveVelocityConstraints()V
    .registers 32

    .prologue
    .line 337
    const/4 v3, 0x0

    :goto_1
    move-object/from16 v0, p0

    iget v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    if-ge v3, v4, :cond_62d

    .line 338
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    aget-object v7, v4, v3

    .line 340
    iget v8, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->indexA:I

    .line 341
    iget v9, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->indexB:I

    .line 343
    iget v10, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invMassA:F

    .line 344
    iget v11, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invMassB:F

    .line 345
    iget v12, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invIA:F

    .line 346
    iget v13, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invIB:F

    .line 347
    iget v14, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->pointCount:I

    .line 349
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v4, v4, v8

    iget-object v15, v4, Lorg/jbox2d/dynamics/contacts/Velocity;->v:Lorg/jbox2d/common/Vec2;

    .line 350
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v4, v4, v8

    iget v6, v4, Lorg/jbox2d/dynamics/contacts/Velocity;->w:F

    .line 351
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v4, v4, v9

    iget-object v0, v4, Lorg/jbox2d/dynamics/contacts/Velocity;->v:Lorg/jbox2d/common/Vec2;

    move-object/from16 v16, v0

    .line 352
    move-object/from16 v0, p0

    iget-object v4, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v4, v4, v9

    iget v5, v4, Lorg/jbox2d/dynamics/contacts/Velocity;->w:F

    .line 354
    iget-object v4, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->normal:Lorg/jbox2d/common/Vec2;

    .line 355
    iget v0, v4, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v17, v0

    .line 356
    iget v0, v4, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v18, v0

    .line 357
    const/high16 v4, 0x3f800000    # 1.0f

    iget-object v0, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->normal:Lorg/jbox2d/common/Vec2;

    move-object/from16 v19, v0

    move-object/from16 v0, v19

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v19, v0

    mul-float v19, v19, v4

    .line 358
    const/high16 v4, -0x40800000    # -1.0f

    iget-object v0, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->normal:Lorg/jbox2d/common/Vec2;

    move-object/from16 v20, v0

    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v20, v0

    mul-float v20, v20, v4

    .line 359
    iget v0, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->friction:F

    move/from16 v21, v0

    .line 361
    sget-boolean v4, Lorg/jbox2d/dynamics/contacts/ContactSolver;->$assertionsDisabled:Z

    if-nez v4, :cond_77

    const/4 v4, 0x1

    if-eq v14, v4, :cond_77

    const/4 v4, 0x2

    if-eq v14, v4, :cond_77

    new-instance v3, Ljava/lang/AssertionError;

    invoke-direct {v3}, Ljava/lang/AssertionError;-><init>()V

    throw v3

    .line 364
    :cond_77
    const/4 v4, 0x0

    :goto_78
    if-ge v4, v14, :cond_19f

    .line 365
    iget-object v0, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->points:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;

    move-object/from16 v22, v0

    aget-object v22, v22, v4

    .line 366
    move-object/from16 v0, v22

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    move-object/from16 v23, v0

    .line 367
    neg-float v0, v5

    move/from16 v24, v0

    move-object/from16 v0, v22

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    move-object/from16 v25, v0

    move-object/from16 v0, v25

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v25, v0

    mul-float v24, v24, v25

    move-object/from16 v0, v16

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v25, v0

    add-float v24, v24, v25

    iget v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v25, v0

    sub-float v24, v24, v25

    move-object/from16 v0, v23

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v25, v0

    mul-float v25, v25, v6

    add-float v24, v24, v25

    .line 368
    move-object/from16 v0, v22

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    move-object/from16 v25, v0

    move-object/from16 v0, v25

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v25, v0

    mul-float v25, v25, v5

    move-object/from16 v0, v16

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v26, v0

    add-float v25, v25, v26

    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v26, v0

    sub-float v25, v25, v26

    move-object/from16 v0, v23

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v23, v0

    mul-float v23, v23, v6

    sub-float v23, v25, v23

    .line 371
    mul-float v24, v24, v19

    mul-float v23, v23, v20

    add-float v23, v23, v24

    iget v0, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->tangentSpeed:F

    move/from16 v24, v0

    sub-float v23, v23, v24

    .line 372
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->tangentMass:F

    move/from16 v24, v0

    move/from16 v0, v23

    neg-float v0, v0

    move/from16 v23, v0

    mul-float v23, v23, v24

    .line 375
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    move/from16 v24, v0

    mul-float v24, v24, v21

    .line 376
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->tangentImpulse:F

    move/from16 v25, v0

    add-float v23, v23, v25

    move/from16 v0, v24

    neg-float v0, v0

    move/from16 v25, v0

    move/from16 v0, v23

    move/from16 v1, v25

    move/from16 v2, v24

    invoke-static {v0, v1, v2}, Lorg/jbox2d/common/MathUtils;->clamp(FFF)F

    move-result v23

    .line 378
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->tangentImpulse:F

    move/from16 v24, v0

    sub-float v24, v23, v24

    .line 379
    move/from16 v0, v23

    move-object/from16 v1, v22

    iput v0, v1, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->tangentImpulse:F

    .line 384
    mul-float v23, v19, v24

    .line 385
    mul-float v24, v24, v20

    .line 388
    iget v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v25, v0

    mul-float v26, v23, v10

    sub-float v25, v25, v26

    move/from16 v0, v25

    iput v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    .line 389
    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v25, v0

    mul-float v26, v24, v10

    sub-float v25, v25, v26

    move/from16 v0, v25

    iput v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    .line 390
    move-object/from16 v0, v22

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    move-object/from16 v25, v0

    move-object/from16 v0, v25

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v25, v0

    mul-float v25, v25, v24

    move-object/from16 v0, v22

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    move-object/from16 v26, v0

    move-object/from16 v0, v26

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v26, v0

    mul-float v26, v26, v23

    sub-float v25, v25, v26

    mul-float v25, v25, v12

    sub-float v6, v6, v25

    .line 393
    move-object/from16 v0, v16

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v25, v0

    mul-float v26, v23, v11

    add-float v25, v25, v26

    move/from16 v0, v25

    move-object/from16 v1, v16

    iput v0, v1, Lorg/jbox2d/common/Vec2;->x:F

    .line 394
    move-object/from16 v0, v16

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v25, v0

    mul-float v26, v24, v11

    add-float v25, v25, v26

    move/from16 v0, v25

    move-object/from16 v1, v16

    iput v0, v1, Lorg/jbox2d/common/Vec2;->y:F

    .line 395
    move-object/from16 v0, v22

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    move-object/from16 v25, v0

    move-object/from16 v0, v25

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v25, v0

    mul-float v24, v24, v25

    move-object/from16 v0, v22

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    move-object/from16 v22, v0

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v22, v0

    mul-float v22, v22, v23

    sub-float v22, v24, v22

    mul-float v22, v22, v13

    add-float v5, v5, v22

    .line 364
    add-int/lit8 v4, v4, 0x1

    goto/16 :goto_78

    .line 399
    :cond_19f
    iget v4, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->pointCount:I

    const/4 v14, 0x1

    if-ne v4, v14, :cond_25b

    .line 400
    iget-object v4, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->points:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;

    const/4 v7, 0x0

    aget-object v7, v4, v7

    .line 405
    neg-float v4, v5

    iget-object v14, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    iget v14, v14, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v4, v14

    move-object/from16 v0, v16

    iget v14, v0, Lorg/jbox2d/common/Vec2;->x:F

    add-float/2addr v4, v14

    iget v14, v15, Lorg/jbox2d/common/Vec2;->x:F

    sub-float/2addr v4, v14

    iget-object v14, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    iget v14, v14, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v14, v6

    add-float/2addr v4, v14

    .line 406
    iget-object v14, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    iget v14, v14, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v14, v5

    move-object/from16 v0, v16

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v19, v0

    add-float v14, v14, v19

    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v19, v0

    sub-float v14, v14, v19

    iget-object v0, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    move-object/from16 v19, v0

    move-object/from16 v0, v19

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v19, v0

    mul-float v19, v19, v6

    sub-float v14, v14, v19

    .line 409
    mul-float v4, v4, v17

    mul-float v14, v14, v18

    add-float/2addr v4, v14

    .line 410
    iget v14, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalMass:F

    neg-float v14, v14

    iget v0, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->velocityBias:F

    move/from16 v19, v0

    sub-float v4, v4, v19

    mul-float/2addr v4, v14

    .line 413
    iget v14, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    add-float/2addr v4, v14

    .line 414
    const/4 v14, 0x0

    cmpl-float v14, v4, v14

    if-lez v14, :cond_259

    .line 415
    :goto_1f5
    iget v14, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    sub-float v14, v4, v14

    .line 416
    iput v4, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    .line 419
    mul-float v4, v17, v14

    .line 420
    mul-float v14, v14, v18

    .line 423
    iget v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v17, v0

    mul-float v18, v4, v10

    sub-float v17, v17, v18

    move/from16 v0, v17

    iput v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    .line 424
    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v17, v0

    mul-float/2addr v10, v14

    sub-float v10, v17, v10

    iput v10, v15, Lorg/jbox2d/common/Vec2;->y:F

    .line 425
    iget-object v10, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    iget v10, v10, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v10, v14

    iget-object v15, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    iget v15, v15, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v15, v4

    sub-float/2addr v10, v15

    mul-float/2addr v10, v12

    sub-float/2addr v6, v10

    .line 428
    move-object/from16 v0, v16

    iget v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v12, v4, v11

    add-float/2addr v10, v12

    move-object/from16 v0, v16

    iput v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    .line 429
    move-object/from16 v0, v16

    iget v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v11, v14

    add-float/2addr v10, v11

    move-object/from16 v0, v16

    iput v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    .line 430
    iget-object v10, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    iget v10, v10, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v10, v14

    iget-object v7, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    iget v7, v7, Lorg/jbox2d/common/Vec2;->y:F

    mul-float/2addr v4, v7

    sub-float v4, v10, v4

    mul-float/2addr v4, v13

    add-float/2addr v4, v5

    move v5, v6

    .line 747
    :goto_245
    move-object/from16 v0, p0

    iget-object v6, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v6, v6, v8

    iput v5, v6, Lorg/jbox2d/dynamics/contacts/Velocity;->w:F

    .line 749
    move-object/from16 v0, p0

    iget-object v5, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v5, v5, v9

    iput v4, v5, Lorg/jbox2d/dynamics/contacts/Velocity;->w:F

    .line 337
    add-int/lit8 v3, v3, 0x1

    goto/16 :goto_1

    .line 414
    :cond_259
    const/4 v4, 0x0

    goto :goto_1f5

    .line 471
    :cond_25b
    iget-object v4, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->points:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;

    const/4 v14, 0x0

    aget-object v14, v4, v14

    .line 472
    iget-object v4, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->points:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;

    const/16 v19, 0x1

    aget-object v19, v4, v19

    .line 473
    iget-object v4, v14, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    .line 474
    iget-object v0, v14, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    move-object/from16 v20, v0

    .line 475
    move-object/from16 v0, v19

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    move-object/from16 v21, v0

    .line 476
    move-object/from16 v0, v19

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    move-object/from16 v22, v0

    .line 477
    iget v0, v14, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    move/from16 v23, v0

    .line 478
    move-object/from16 v0, v19

    iget v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    move/from16 v24, v0

    .line 480
    sget-boolean v25, Lorg/jbox2d/dynamics/contacts/ContactSolver;->$assertionsDisabled:Z

    if-nez v25, :cond_298

    const/16 v25, 0x0

    cmpl-float v25, v23, v25

    if-ltz v25, :cond_292

    const/16 v25, 0x0

    cmpl-float v25, v24, v25

    if-gez v25, :cond_298

    :cond_292
    new-instance v3, Ljava/lang/AssertionError;

    invoke-direct {v3}, Ljava/lang/AssertionError;-><init>()V

    throw v3

    .line 483
    :cond_298
    neg-float v0, v5

    move/from16 v25, v0

    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v26, v0

    mul-float v25, v25, v26

    move-object/from16 v0, v16

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v26, v0

    add-float v25, v25, v26

    iget v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v26, v0

    sub-float v25, v25, v26

    iget v0, v4, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v26, v0

    mul-float v26, v26, v6

    add-float v25, v25, v26

    .line 484
    move-object/from16 v0, v20

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v26, v0

    mul-float v26, v26, v5

    move-object/from16 v0, v16

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v27, v0

    add-float v26, v26, v27

    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v27, v0

    sub-float v26, v26, v27

    iget v0, v4, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v27, v0

    mul-float v27, v27, v6

    sub-float v26, v26, v27

    .line 487
    neg-float v0, v5

    move/from16 v27, v0

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v28, v0

    mul-float v27, v27, v28

    move-object/from16 v0, v16

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v28, v0

    add-float v27, v27, v28

    iget v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v28, v0

    sub-float v27, v27, v28

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v28, v0

    mul-float v28, v28, v6

    add-float v27, v27, v28

    .line 488
    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v28, v0

    mul-float v28, v28, v5

    move-object/from16 v0, v16

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v29, v0

    add-float v28, v28, v29

    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v29, v0

    sub-float v28, v28, v29

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v29, v0

    mul-float v29, v29, v6

    sub-float v28, v28, v29

    .line 491
    mul-float v25, v25, v17

    mul-float v26, v26, v18

    add-float v25, v25, v26

    .line 492
    mul-float v26, v27, v17

    mul-float v27, v28, v18

    add-float v26, v26, v27

    .line 494
    iget v0, v14, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->velocityBias:F

    move/from16 v27, v0

    sub-float v25, v25, v27

    .line 495
    move-object/from16 v0, v19

    iget v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->velocityBias:F

    move/from16 v27, v0

    sub-float v26, v26, v27

    .line 498
    iget-object v0, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->K:Lorg/jbox2d/common/Mat22;

    move-object/from16 v27, v0

    .line 499
    move-object/from16 v0, v27

    iget-object v0, v0, Lorg/jbox2d/common/Mat22;->ex:Lorg/jbox2d/common/Vec2;

    move-object/from16 v28, v0

    move-object/from16 v0, v28

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v28, v0

    mul-float v28, v28, v23

    move-object/from16 v0, v27

    iget-object v0, v0, Lorg/jbox2d/common/Mat22;->ey:Lorg/jbox2d/common/Vec2;

    move-object/from16 v29, v0

    move-object/from16 v0, v29

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v29, v0

    mul-float v29, v29, v24

    add-float v28, v28, v29

    sub-float v25, v25, v28

    .line 500
    move-object/from16 v0, v27

    iget-object v0, v0, Lorg/jbox2d/common/Mat22;->ex:Lorg/jbox2d/common/Vec2;

    move-object/from16 v28, v0

    move-object/from16 v0, v28

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v28, v0

    mul-float v28, v28, v23

    move-object/from16 v0, v27

    iget-object v0, v0, Lorg/jbox2d/common/Mat22;->ey:Lorg/jbox2d/common/Vec2;

    move-object/from16 v27, v0

    move-object/from16 v0, v27

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v27, v0

    mul-float v27, v27, v24

    add-float v27, v27, v28

    sub-float v26, v26, v27

    .line 515
    iget-object v0, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->normalMass:Lorg/jbox2d/common/Mat22;

    move-object/from16 v27, v0

    .line 516
    move-object/from16 v0, v27

    iget-object v0, v0, Lorg/jbox2d/common/Mat22;->ex:Lorg/jbox2d/common/Vec2;

    move-object/from16 v28, v0

    move-object/from16 v0, v28

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v28, v0

    mul-float v28, v28, v25

    move-object/from16 v0, v27

    iget-object v0, v0, Lorg/jbox2d/common/Mat22;->ey:Lorg/jbox2d/common/Vec2;

    move-object/from16 v29, v0

    move-object/from16 v0, v29

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v29, v0

    mul-float v29, v29, v26

    add-float v28, v28, v29

    .line 517
    move-object/from16 v0, v27

    iget-object v0, v0, Lorg/jbox2d/common/Mat22;->ex:Lorg/jbox2d/common/Vec2;

    move-object/from16 v29, v0

    move-object/from16 v0, v29

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v29, v0

    mul-float v29, v29, v25

    move-object/from16 v0, v27

    iget-object v0, v0, Lorg/jbox2d/common/Mat22;->ey:Lorg/jbox2d/common/Vec2;

    move-object/from16 v27, v0

    move-object/from16 v0, v27

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v27, v0

    mul-float v27, v27, v26

    add-float v27, v27, v29

    .line 518
    const/high16 v29, -0x40800000    # -1.0f

    mul-float v28, v28, v29

    .line 519
    const/high16 v29, -0x40800000    # -1.0f

    mul-float v27, v27, v29

    .line 521
    const/16 v29, 0x0

    cmpl-float v29, v28, v29

    if-ltz v29, :cond_44c

    const/16 v29, 0x0

    cmpl-float v29, v27, v29

    if-ltz v29, :cond_44c

    .line 524
    sub-float v7, v28, v23

    .line 525
    sub-float v23, v27, v24

    .line 530
    mul-float v24, v7, v17

    .line 531
    mul-float v7, v7, v18

    .line 532
    mul-float v17, v17, v23

    .line 533
    mul-float v18, v18, v23

    .line 541
    iget v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v23, v0

    add-float v25, v24, v17

    mul-float v25, v25, v10

    sub-float v23, v23, v25

    move/from16 v0, v23

    iput v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    .line 542
    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v23, v0

    add-float v25, v7, v18

    mul-float v10, v10, v25

    sub-float v10, v23, v10

    iput v10, v15, Lorg/jbox2d/common/Vec2;->y:F

    .line 543
    move-object/from16 v0, v16

    iget v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    add-float v15, v24, v17

    mul-float/2addr v15, v11

    add-float/2addr v10, v15

    move-object/from16 v0, v16

    iput v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    .line 544
    move-object/from16 v0, v16

    iget v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    add-float v15, v7, v18

    mul-float/2addr v11, v15

    add-float/2addr v10, v11

    move-object/from16 v0, v16

    iput v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    .line 546
    iget v10, v4, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v10, v7

    iget v4, v4, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v4, v4, v24

    sub-float v4, v10, v4

    move-object/from16 v0, v21

    iget v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v10, v10, v18

    move-object/from16 v0, v21

    iget v11, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v11, v11, v17

    sub-float/2addr v10, v11

    add-float/2addr v4, v10

    mul-float/2addr v4, v12

    sub-float/2addr v6, v4

    .line 547
    move-object/from16 v0, v20

    iget v4, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v4, v7

    move-object/from16 v0, v20

    iget v7, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v7, v7, v24

    sub-float/2addr v4, v7

    move-object/from16 v0, v22

    iget v7, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v7, v7, v18

    move-object/from16 v0, v22

    iget v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v10, v10, v17

    sub-float/2addr v7, v10

    add-float/2addr v4, v7

    mul-float/2addr v4, v13

    add-float/2addr v4, v5

    .line 550
    move/from16 v0, v28

    iput v0, v14, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    .line 551
    move/from16 v0, v27

    move-object/from16 v1, v19

    iput v0, v1, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    move v5, v6

    .line 573
    goto/16 :goto_245

    .line 582
    :cond_44c
    iget v0, v14, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalMass:F

    move/from16 v27, v0

    move/from16 v0, v27

    neg-float v0, v0

    move/from16 v27, v0

    mul-float v27, v27, v25

    .line 583
    const/16 v28, 0x0

    .line 585
    iget-object v0, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->K:Lorg/jbox2d/common/Mat22;

    move-object/from16 v29, v0

    move-object/from16 v0, v29

    iget-object v0, v0, Lorg/jbox2d/common/Mat22;->ex:Lorg/jbox2d/common/Vec2;

    move-object/from16 v29, v0

    move-object/from16 v0, v29

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v29, v0

    mul-float v29, v29, v27

    add-float v29, v29, v26

    .line 587
    const/16 v30, 0x0

    cmpl-float v30, v27, v30

    if-ltz v30, :cond_4f9

    const/16 v30, 0x0

    cmpl-float v29, v29, v30

    if-ltz v29, :cond_4f9

    .line 589
    sub-float v7, v27, v23

    .line 590
    sub-float v23, v28, v24

    .line 595
    mul-float v24, v17, v7

    .line 596
    mul-float v7, v7, v18

    .line 597
    mul-float v17, v17, v23

    .line 598
    mul-float v18, v18, v23

    .line 607
    iget v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v23, v0

    add-float v25, v24, v17

    mul-float v25, v25, v10

    sub-float v23, v23, v25

    move/from16 v0, v23

    iput v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    .line 608
    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v23, v0

    add-float v25, v7, v18

    mul-float v10, v10, v25

    sub-float v10, v23, v10

    iput v10, v15, Lorg/jbox2d/common/Vec2;->y:F

    .line 609
    move-object/from16 v0, v16

    iget v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    add-float v15, v24, v17

    mul-float/2addr v15, v11

    add-float/2addr v10, v15

    move-object/from16 v0, v16

    iput v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    .line 610
    move-object/from16 v0, v16

    iget v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    add-float v15, v7, v18

    mul-float/2addr v11, v15

    add-float/2addr v10, v11

    move-object/from16 v0, v16

    iput v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    .line 612
    iget v10, v4, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v10, v7

    iget v4, v4, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v4, v4, v24

    sub-float v4, v10, v4

    move-object/from16 v0, v21

    iget v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v10, v10, v18

    move-object/from16 v0, v21

    iget v11, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v11, v11, v17

    sub-float/2addr v10, v11

    add-float/2addr v4, v10

    mul-float/2addr v4, v12

    sub-float/2addr v6, v4

    .line 613
    move-object/from16 v0, v20

    iget v4, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v4, v7

    move-object/from16 v0, v20

    iget v7, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v7, v7, v24

    sub-float/2addr v4, v7

    move-object/from16 v0, v22

    iget v7, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v7, v7, v18

    move-object/from16 v0, v22

    iget v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v10, v10, v17

    sub-float/2addr v7, v10

    add-float/2addr v4, v7

    mul-float/2addr v4, v13

    add-float/2addr v4, v5

    .line 616
    move/from16 v0, v27

    iput v0, v14, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    .line 617
    move/from16 v0, v28

    move-object/from16 v1, v19

    iput v0, v1, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    move v5, v6

    .line 635
    goto/16 :goto_245

    .line 644
    :cond_4f9
    const/16 v27, 0x0

    .line 645
    move-object/from16 v0, v19

    iget v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalMass:F

    move/from16 v28, v0

    move/from16 v0, v28

    neg-float v0, v0

    move/from16 v28, v0

    mul-float v28, v28, v26

    .line 646
    iget-object v7, v7, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->K:Lorg/jbox2d/common/Mat22;

    iget-object v7, v7, Lorg/jbox2d/common/Mat22;->ey:Lorg/jbox2d/common/Vec2;

    iget v7, v7, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v7, v7, v28

    add-float v7, v7, v25

    .line 649
    const/16 v29, 0x0

    cmpl-float v29, v28, v29

    if-ltz v29, :cond_59e

    const/16 v29, 0x0

    cmpl-float v7, v7, v29

    if-ltz v7, :cond_59e

    .line 651
    sub-float v7, v27, v23

    .line 652
    sub-float v23, v28, v24

    .line 662
    mul-float v24, v17, v7

    .line 663
    mul-float v7, v7, v18

    .line 664
    mul-float v17, v17, v23

    .line 665
    mul-float v18, v18, v23

    .line 667
    iget v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v23, v0

    add-float v25, v24, v17

    mul-float v25, v25, v10

    sub-float v23, v23, v25

    move/from16 v0, v23

    iput v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    .line 668
    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v23, v0

    add-float v25, v7, v18

    mul-float v10, v10, v25

    sub-float v10, v23, v10

    iput v10, v15, Lorg/jbox2d/common/Vec2;->y:F

    .line 669
    move-object/from16 v0, v16

    iget v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    add-float v15, v24, v17

    mul-float/2addr v15, v11

    add-float/2addr v10, v15

    move-object/from16 v0, v16

    iput v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    .line 670
    move-object/from16 v0, v16

    iget v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    add-float v15, v7, v18

    mul-float/2addr v11, v15

    add-float/2addr v10, v11

    move-object/from16 v0, v16

    iput v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    .line 672
    iget v10, v4, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v10, v7

    iget v4, v4, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v4, v4, v24

    sub-float v4, v10, v4

    move-object/from16 v0, v21

    iget v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v10, v10, v18

    move-object/from16 v0, v21

    iget v11, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v11, v11, v17

    sub-float/2addr v10, v11

    add-float/2addr v4, v10

    mul-float/2addr v4, v12

    sub-float/2addr v6, v4

    .line 673
    move-object/from16 v0, v20

    iget v4, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float/2addr v4, v7

    move-object/from16 v0, v20

    iget v7, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v7, v7, v24

    sub-float/2addr v4, v7

    move-object/from16 v0, v22

    iget v7, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v7, v7, v18

    move-object/from16 v0, v22

    iget v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v10, v10, v17

    sub-float/2addr v7, v10

    add-float/2addr v4, v7

    mul-float/2addr v4, v13

    add-float/2addr v4, v5

    .line 676
    move/from16 v0, v27

    iput v0, v14, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    .line 677
    move/from16 v0, v28

    move-object/from16 v1, v19

    iput v0, v1, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    move v5, v6

    .line 695
    goto/16 :goto_245

    .line 703
    :cond_59e
    const/4 v7, 0x0

    .line 704
    const/16 v27, 0x0

    .line 708
    const/16 v28, 0x0

    cmpl-float v25, v25, v28

    if-ltz v25, :cond_62e

    const/16 v25, 0x0

    cmpl-float v25, v26, v25

    if-ltz v25, :cond_62e

    .line 710
    sub-float v23, v7, v23

    .line 711
    sub-float v24, v27, v24

    .line 721
    mul-float v25, v17, v23

    .line 722
    mul-float v23, v23, v18

    .line 723
    mul-float v17, v17, v24

    .line 724
    mul-float v18, v18, v24

    .line 726
    iget v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v24, v0

    add-float v26, v25, v17

    mul-float v26, v26, v10

    sub-float v24, v24, v26

    move/from16 v0, v24

    iput v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    .line 727
    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v24, v0

    add-float v26, v23, v18

    mul-float v10, v10, v26

    sub-float v10, v24, v10

    iput v10, v15, Lorg/jbox2d/common/Vec2;->y:F

    .line 728
    move-object/from16 v0, v16

    iget v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    add-float v15, v25, v17

    mul-float/2addr v15, v11

    add-float/2addr v10, v15

    move-object/from16 v0, v16

    iput v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    .line 729
    move-object/from16 v0, v16

    iget v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    add-float v15, v23, v18

    mul-float/2addr v11, v15

    add-float/2addr v10, v11

    move-object/from16 v0, v16

    iput v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    .line 731
    iget v10, v4, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v10, v10, v23

    iget v4, v4, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v4, v4, v25

    sub-float v4, v10, v4

    move-object/from16 v0, v21

    iget v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v10, v10, v18

    move-object/from16 v0, v21

    iget v11, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v11, v11, v17

    sub-float/2addr v10, v11

    add-float/2addr v4, v10

    mul-float/2addr v4, v12

    sub-float/2addr v6, v4

    .line 732
    move-object/from16 v0, v20

    iget v4, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v4, v4, v23

    move-object/from16 v0, v20

    iget v10, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v10, v10, v25

    sub-float/2addr v4, v10

    move-object/from16 v0, v22

    iget v10, v0, Lorg/jbox2d/common/Vec2;->x:F

    mul-float v10, v10, v18

    move-object/from16 v0, v22

    iget v11, v0, Lorg/jbox2d/common/Vec2;->y:F

    mul-float v11, v11, v17

    sub-float/2addr v10, v11

    add-float/2addr v4, v10

    mul-float/2addr v4, v13

    add-float/2addr v4, v5

    .line 735
    iput v7, v14, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    .line 736
    move/from16 v0, v27

    move-object/from16 v1, v19

    iput v0, v1, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    move v5, v6

    .line 738
    goto/16 :goto_245

    .line 751
    :cond_62d
    return-void

    :cond_62e
    move v4, v5

    move v5, v6

    goto/16 :goto_245
.end method

.method public storeImpulses()V
    .registers 8

    .prologue
    const/4 v1, 0x0

    .line 754
    move v0, v1

    :goto_2
    iget v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    if-ge v0, v2, :cond_37

    .line 755
    iget-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    aget-object v3, v2, v0

    .line 756
    iget-object v2, p0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_contacts:[Lorg/jbox2d/dynamics/contacts/Contact;

    iget v4, v3, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->contactIndex:I

    aget-object v2, v2, v4

    invoke-virtual {v2}, Lorg/jbox2d/dynamics/contacts/Contact;->getManifold()Lorg/jbox2d/collision/Manifold;

    move-result-object v4

    move v2, v1

    .line 758
    :goto_15
    iget v5, v3, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->pointCount:I

    if-ge v2, v5, :cond_34

    .line 759
    iget-object v5, v4, Lorg/jbox2d/collision/Manifold;->points:[Lorg/jbox2d/collision/ManifoldPoint;

    aget-object v5, v5, v2

    iget-object v6, v3, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->points:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;

    aget-object v6, v6, v2

    iget v6, v6, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    iput v6, v5, Lorg/jbox2d/collision/ManifoldPoint;->normalImpulse:F

    .line 760
    iget-object v5, v4, Lorg/jbox2d/collision/Manifold;->points:[Lorg/jbox2d/collision/ManifoldPoint;

    aget-object v5, v5, v2

    iget-object v6, v3, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->points:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;

    aget-object v6, v6, v2

    iget v6, v6, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->tangentImpulse:F

    iput v6, v5, Lorg/jbox2d/collision/ManifoldPoint;->tangentImpulse:F

    .line 758
    add-int/lit8 v2, v2, 0x1

    goto :goto_15

    .line 754
    :cond_34
    add-int/lit8 v0, v0, 0x1

    goto :goto_2

    .line 763
    :cond_37
    return-void
.end method

.method public warmStart()V
    .registers 24

    .prologue
    .line 179
    const/4 v1, 0x0

    :goto_1
    move-object/from16 v0, p0

    iget v2, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_count:I

    if-ge v1, v2, :cond_10e

    .line 180
    move-object/from16 v0, p0

    iget-object v2, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocityConstraints:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;

    aget-object v5, v2, v1

    .line 182
    iget v6, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->indexA:I

    .line 183
    iget v7, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->indexB:I

    .line 184
    iget v8, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invMassA:F

    .line 185
    iget v9, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invIA:F

    .line 186
    iget v10, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invMassB:F

    .line 187
    iget v11, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->invIB:F

    .line 188
    iget v12, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->pointCount:I

    .line 190
    move-object/from16 v0, p0

    iget-object v2, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v2, v2, v6

    iget-object v13, v2, Lorg/jbox2d/dynamics/contacts/Velocity;->v:Lorg/jbox2d/common/Vec2;

    .line 191
    move-object/from16 v0, p0

    iget-object v2, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v2, v2, v6

    iget v4, v2, Lorg/jbox2d/dynamics/contacts/Velocity;->w:F

    .line 192
    move-object/from16 v0, p0

    iget-object v2, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v2, v2, v7

    iget-object v14, v2, Lorg/jbox2d/dynamics/contacts/Velocity;->v:Lorg/jbox2d/common/Vec2;

    .line 193
    move-object/from16 v0, p0

    iget-object v2, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v2, v2, v7

    iget v3, v2, Lorg/jbox2d/dynamics/contacts/Velocity;->w:F

    .line 195
    iget-object v15, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->normal:Lorg/jbox2d/common/Vec2;

    .line 196
    const/high16 v2, 0x3f800000    # 1.0f

    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v16, v0

    mul-float v16, v16, v2

    .line 197
    const/high16 v2, -0x40800000    # -1.0f

    iget v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v17, v0

    mul-float v17, v17, v2

    .line 199
    const/4 v2, 0x0

    :goto_4e
    if-ge v2, v12, :cond_fa

    .line 200
    iget-object v0, v5, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint;->points:[Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;

    move-object/from16 v18, v0

    aget-object v18, v18, v2

    .line 201
    move-object/from16 v0, v18

    iget v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->tangentImpulse:F

    move/from16 v19, v0

    mul-float v19, v19, v16

    iget v0, v15, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v20, v0

    move-object/from16 v0, v18

    iget v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    move/from16 v21, v0

    mul-float v20, v20, v21

    add-float v19, v19, v20

    .line 202
    move-object/from16 v0, v18

    iget v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->tangentImpulse:F

    move/from16 v20, v0

    mul-float v20, v20, v17

    iget v0, v15, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v21, v0

    move-object/from16 v0, v18

    iget v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->normalImpulse:F

    move/from16 v22, v0

    mul-float v21, v21, v22

    add-float v20, v20, v21

    .line 204
    move-object/from16 v0, v18

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    move-object/from16 v21, v0

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v21, v0

    mul-float v21, v21, v20

    move-object/from16 v0, v18

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rA:Lorg/jbox2d/common/Vec2;

    move-object/from16 v22, v0

    move-object/from16 v0, v22

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v22, v0

    mul-float v22, v22, v19

    sub-float v21, v21, v22

    mul-float v21, v21, v9

    sub-float v4, v4, v21

    .line 205
    iget v0, v13, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v21, v0

    mul-float v22, v19, v8

    sub-float v21, v21, v22

    move/from16 v0, v21

    iput v0, v13, Lorg/jbox2d/common/Vec2;->x:F

    .line 206
    iget v0, v13, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v21, v0

    mul-float v22, v20, v8

    sub-float v21, v21, v22

    move/from16 v0, v21

    iput v0, v13, Lorg/jbox2d/common/Vec2;->y:F

    .line 207
    move-object/from16 v0, v18

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    move-object/from16 v21, v0

    move-object/from16 v0, v21

    iget v0, v0, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v21, v0

    mul-float v21, v21, v20

    move-object/from16 v0, v18

    iget-object v0, v0, Lorg/jbox2d/dynamics/contacts/ContactVelocityConstraint$VelocityConstraintPoint;->rB:Lorg/jbox2d/common/Vec2;

    move-object/from16 v18, v0

    move-object/from16 v0, v18

    iget v0, v0, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v18, v0

    mul-float v18, v18, v19

    sub-float v18, v21, v18

    mul-float v18, v18, v11

    add-float v3, v3, v18

    .line 208
    iget v0, v14, Lorg/jbox2d/common/Vec2;->x:F

    move/from16 v18, v0

    mul-float v19, v19, v10

    add-float v18, v18, v19

    move/from16 v0, v18

    iput v0, v14, Lorg/jbox2d/common/Vec2;->x:F

    .line 209
    iget v0, v14, Lorg/jbox2d/common/Vec2;->y:F

    move/from16 v18, v0

    mul-float v19, v20, v10

    add-float v18, v18, v19

    move/from16 v0, v18

    iput v0, v14, Lorg/jbox2d/common/Vec2;->y:F

    .line 199
    add-int/lit8 v2, v2, 0x1

    goto/16 :goto_4e

    .line 211
    :cond_fa
    move-object/from16 v0, p0

    iget-object v2, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v2, v2, v6

    iput v4, v2, Lorg/jbox2d/dynamics/contacts/Velocity;->w:F

    .line 212
    move-object/from16 v0, p0

    iget-object v2, v0, Lorg/jbox2d/dynamics/contacts/ContactSolver;->m_velocities:[Lorg/jbox2d/dynamics/contacts/Velocity;

    aget-object v2, v2, v7

    iput v3, v2, Lorg/jbox2d/dynamics/contacts/Velocity;->w:F

    .line 179
    add-int/lit8 v1, v1, 0x1

    goto/16 :goto_1

    .line 214
    :cond_10e
    return-void
.end method