Repository: Genesis-Embodied-AI/Genesis
File: genesis/engine/solvers/rigid/constraint_noslip.py (Python)

Code before:

    ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
    for i_b in range(_B):

        # temp variables
        res = ti.Vector.zero(gs.ti_float, 5)
        old_force = ti.Vector.zero(gs.ti_float, 5)
        bc = ti.Vector.zero(gs.ti_float, 5)
        Ac = ti.Vector.zero(gs.ti_float, 9)

        n_con = collider_state.n_contacts[i_b]
        ne = constraint_state.n_constraints_equality[i_b]
        nf = constraint_state.n_constraints_frictionloss[i_b]
        const_start = ne + nf

        scale = 1.0 / (rigid_global_info.meaninertia[i_b] * ti.max(1.0, n_dofs))

        for i_iter in range(static_rigid_sim_config.noslip_iterations):

            improvement = gs.ti_float(0.0)
            if i_iter == 0:
                for i_c in range(constraint_state.n_constraints[i_b]):
                    improvement += (
                        0.5
                        * constraint_state.efc_force[i_c, i_b]
                        * constraint_state.efc_force[i_c, i_b]
                        / constraint_state.efc_D[i_c, i_b]
                    )

            for i_c in range(ne, ne + nf):
                res = func_residual(
                    res=res,
                    i_b=i_b,
                    i_efc=i_c,
                    dim=1,
                    flg_subR=True,
                    constraint_state=constraint_state,
                )
                old_force[0] = constraint_state.efc_force[i_c, i_b]
                constraint_state.efc_force[i_c, i_b] -= res[0] / constraint_state.efc_AR[i_c, i_c, i_b]
                if constraint_state.efc_force[i_c, i_b] < -constraint_state.efc_frictionloss[i_c, i_b]:
                    constraint_state.efc_force[i_c, i_b] = -constraint_state.efc_frictionloss[i_c, i_b]
                elif constraint_state.efc_force[i_c, i_b] > constraint_state.efc_frictionloss[i_c, i_b]:
                    constraint_state.efc_force[i_c, i_b] = constraint_state.efc_frictionloss[i_c, i_b]
                delta = constraint_state.efc_force[i_c, i_b] - old_force[0]
                improvement -= 0.5 * delta * delta / constraint_state.efc_AR[i_c, i_c, i_b] + delta * res[0]

            # Project contact friction (pyramidal 4-edge) with normal fixed
            for i_col in range(n_con):
                base = const_start + i_col * 4
                mu = collider_state.contact_data.friction[i_col, i_b]

Review comment (performance):
just use `constraint_state.efc_force[i_c, i_b] ** 2`, it will be fine.

You may use `* constraint_state.diag[i_c, i_b]` in place of `/ constraint_state.efc_D[i_c, i_b]` to avoid division, which is much slower than multiplication.

Code after:
        res = ti.Vector.zero(gs.ti_float, 5)
        old_force = ti.Vector.zero(gs.ti_float, 5)
        bc = ti.Vector.zero(gs.ti_float, 5)
        Ac = ti.Vector.zero(gs.ti_float, 9)

        n_con = collider_state.n_contacts[i_b]
        ne = constraint_state.n_constraints_equality[i_b]
        nf = constraint_state.n_constraints_frictionloss[i_b]
        const_start = ne + nf

        scale = 1.0 / (rigid_global_info.meaninertia[i_b] * ti.max(1.0, n_dofs))

        for i_iter in range(static_rigid_sim_config.noslip_iterations):

            improvement = gs.ti_float(0.0)
            if i_iter == 0:
                for i_c in range(constraint_state.n_constraints[i_b]):
                    improvement += 0.5 * constraint_state.efc_force[i_c, i_b] ** 2 * constraint_state.diag[i_c, i_b]

            for i_c in range(ne, ne + nf):
                res = func_residual_constraint_force(
                    res=res,
                    i_b=i_b,
                    i_efc=i_c,
                    dim=1,
                    constraint_state=constraint_state,
                )
                old_force[0] = constraint_state.efc_force[i_c, i_b]
                constraint_state.efc_force[i_c, i_b] -= res[0] / constraint_state.efc_AR[i_c, i_c, i_b]
                if constraint_state.efc_force[i_c, i_b] < -constraint_state.efc_frictionloss[i_c, i_b]:
                    constraint_state.efc_force[i_c, i_b] = -constraint_state.efc_frictionloss[i_c, i_b]
                elif constraint_state.efc_force[i_c, i_b] > constraint_state.efc_frictionloss[i_c, i_b]:
                    constraint_state.efc_force[i_c, i_b] = constraint_state.efc_frictionloss[i_c, i_b]
                delta = constraint_state.efc_force[i_c, i_b] - old_force[0]
                improvement -= 0.5 * delta**2 / constraint_state.efc_AR[i_c, i_c, i_b] + delta * res[0]

            # Project contact friction (pyramidal 4-edge) with normal fixed
            for i_col in range(n_con):
                base = const_start + i_col * 4
                mu = collider_state.contact_data.friction[i_col, i_b]
                for j2 in ti.static(range(2)):
                    j_efc = base + j2 * 2
                    res = func_residual_constraint_force(
                        res=res,
                        i_b=i_b,
                        i_efc=j_efc,
                        dim=2,
                        constraint_state=constraint_state,
                    )
                    for i2 in ti.static(range(2)):
                        old_force[i2] = constraint_state.efc_force[j_efc + i2, i_b]