Examples with Transform spacegraph.space3d.phys.math.Transform used on opensource projects

Example 11 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class ConeTwistConstraint method solveConstraint.

@Override
public void solveConstraint(float timeStep) {
    v3 tmp = new v3();
    v3 tmp2 = new v3();
    v3 tmpVec = new v3();
    Transform tmpTrans = new Transform();
    v3 pivotAInW = new v3(rbAFrame);
    rbA.getCenterOfMassTransform(tmpTrans).transform(pivotAInW);
    v3 pivotBInW = new v3(rbBFrame);
    rbB.getCenterOfMassTransform(tmpTrans).transform(pivotBInW);
    float tau = 0.3f;
    // linear part
    if (!angularOnly) {
        v3 rel_pos1 = new v3();
        rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
        v3 rel_pos2 = new v3();
        rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
        v3 vel1 = rbA.getVelocityInLocalPoint(rel_pos1, new v3());
        v3 vel2 = rbB.getVelocityInLocalPoint(rel_pos2, new v3());
        v3 vel = new v3();
        vel.sub(vel1, vel2);
        for (int i = 0; i < 3; i++) {
            v3 normal = jac[i].linearJointAxis;
            float jacDiagABInv = 1f / jac[i].Adiag;
            float rel_vel;
            rel_vel = normal.dot(vel);
            // positional error (zeroth order error)
            tmp.sub(pivotAInW, pivotBInW);
            // this is the error projected on the normal
            float depth = -(tmp).dot(normal);
            float impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
            appliedImpulse += impulse;
            v3 impulse_vector = new v3();
            impulse_vector.scale(impulse, normal);
            tmp.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));
            rbA.impulse(impulse_vector, tmp);
            tmp.negate(impulse_vector);
            tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));
            rbB.impulse(tmp, tmp2);
        }
    }
    // solve angular part
    v3 angVelA = getRigidBodyA().getAngularVelocity(new v3());
    v3 angVelB = getRigidBodyB().getAngularVelocity(new v3());
    // solve swing limit
    if (solveSwingLimit) {
        tmp.sub(angVelB, angVelA);
        float amplitude = ((tmp).dot(swingAxis) * relaxationFactor * relaxationFactor + swingCorrection * (1f / timeStep) * biasFactor);
        float impulseMag = amplitude * kSwing;
        // Clamp the accumulated impulse
        float temp = accSwingLimitImpulse;
        accSwingLimitImpulse = Math.max(accSwingLimitImpulse + impulseMag, 0.0f);
        impulseMag = accSwingLimitImpulse - temp;
        v3 impulse = new v3();
        impulse.scale(impulseMag, swingAxis);
        rbA.torqueImpulse(impulse);
        tmp.negate(impulse);
        rbB.torqueImpulse(tmp);
    }
    // solve twist limit
    if (solveTwistLimit) {
        tmp.sub(angVelB, angVelA);
        float amplitude = ((tmp).dot(twistAxis) * relaxationFactor * relaxationFactor + twistCorrection * (1f / timeStep) * biasFactor);
        float impulseMag = amplitude * kTwist;
        // Clamp the accumulated impulse
        float temp = accTwistLimitImpulse;
        accTwistLimitImpulse = Math.max(accTwistLimitImpulse + impulseMag, 0.0f);
        impulseMag = accTwistLimitImpulse - temp;
        v3 impulse = new v3();
        impulse.scale(impulseMag, twistAxis);
        rbA.torqueImpulse(impulse);
        tmp.negate(impulse);
        rbB.torqueImpulse(tmp);
    }
}

Example 12 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class DistanceConstraint method solveConstraint.

@Override
public void solveConstraint(float timeStep) {
    v3 tmp = new v3();
    v3 tmp2 = new v3();
    // v3 tmpVec = new v3();
    Transform centerOfMassA = rbA.getCenterOfMassTransform(new Transform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(new Transform());
    v3 pivotAInW = new v3(pivotInA);
    centerOfMassA.transform(pivotAInW);
    v3 pivotBInW = new v3(pivotInB);
    centerOfMassB.transform(pivotBInW);
    // v3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
    // v3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
    v3 rel_pos1 = new v3();
    rel_pos1.sub(pivotAInW, centerOfMassA);
    v3 rel_pos2 = new v3();
    rel_pos2.sub(pivotBInW, centerOfMassB);
    v3 vel1 = rbA.getVelocityInLocalPoint(rel_pos1, new v3());
    v3 vel2 = rbB.getVelocityInLocalPoint(rel_pos2, new v3());
    v3 vel = new v3();
    vel.sub(vel1, vel2);
    v3 normal = jac.linearJointAxis;
    float rel_vel = vel.dot(normal);
    /*
            //velocity error (first order error)
			btScalar rel_vel = m_jac.getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
			m_rbB.getLinearVelocity(),angvelB);
			 */
    // positional error (zeroth order error)
    tmp.sub(pivotAInW, pivotBInW);
    // this is the error projected on the normal
    float depth = -tmp.dot(normal);
    float impulse = error * ((depth * tau / timeStep) - (damping * rel_vel));
    float impulseClamp = this.impulseClamp;
    if (impulseClamp > 0f) {
        if (impulse < -impulseClamp) {
            impulse = -impulseClamp;
        }
        if (impulse > impulseClamp) {
            impulse = impulseClamp;
        }
    }
    appliedImpulse += impulse;
    v3 impulse_vector = new v3();
    impulse_vector.scale(impulse, normal);
    tmp.sub(pivotAInW, centerOfMassA);
    rbA.impulse(impulse_vector, tmp);
    tmp.negate(impulse_vector);
    tmp2.sub(pivotBInW, centerOfMassB);
    rbB.impulse(tmp, tmp2);
}

Example 13 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class DistanceConstraint method buildJacobian.

@Override
public void buildJacobian() {
    appliedImpulse = 0;
    Transform posA = rbA.getCenterOfMassTransform(new Transform());
    Transform posB = rbB.getCenterOfMassTransform(new Transform());
    v3 relA = new v3(pivotInA);
    posA.transform(relA);
    v3 relB = new v3(pivotInB);
    posB.transform(relB);
    v3 del = new v3();
    del.sub(posB, posA);
    float currDist = (float) Math.sqrt(del.dot(del));
    v3 ortho = del;
    ortho.scale(1f / currDist);
    Matrix3f tmpMat1 = new Matrix3f(), tmpMat2 = new Matrix3f();
    tmpMat1.transpose(posA.basis);
    tmpMat2.transpose(posB.basis);
    v3 tmp1 = v(pivotInA), tmp2 = v(pivotInB);
    posA.transform(tmp1);
    tmp1.sub(rbA.getCenterOfMassPosition(v()));
    posB.transform(tmp2);
    tmp2.sub(rbB.getCenterOfMassPosition(v()));
    jac.init(tmpMat1, tmpMat2, tmp1, tmp2, ortho, rbA.getInvInertiaDiagLocal(new v3()), rbA.getInvMass(), rbB.getInvInertiaDiagLocal(new v3()), rbB.getInvMass());
    this.error = (currDist - dist) * speed;
// System.out.println("dist=" + (currDist - dist) + " " + jac.linearJointAxis);
}

Example 14 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class Point2PointConstraint method solveConstraint.

@Override
public void solveConstraint(float timeStep) {
    v3 tmp = new v3();
    v3 tmp2 = new v3();
    // v3 tmpVec = new v3();
    Transform centerOfMassA = rbA.getCenterOfMassTransform(new Transform());
    Transform centerOfMassB = rbB.getCenterOfMassTransform(new Transform());
    v3 pivotAInW = new v3(pivotInA);
    centerOfMassA.transform(pivotAInW);
    v3 pivotBInW = new v3(pivotInB);
    centerOfMassB.transform(pivotBInW);
    v3 normal = new v3();
    normal.set(0f, 0f, 0f);
    // btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
    // btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
    v3 rel_pos1 = new v3();
    rel_pos1.sub(pivotAInW, centerOfMassA);
    v3 rel_pos2 = new v3();
    rel_pos2.sub(pivotBInW, centerOfMassB);
    v3 vel1 = rbA.getVelocityInLocalPoint(rel_pos1, new v3());
    v3 vel2 = rbB.getVelocityInLocalPoint(rel_pos2, new v3());
    v3 vel = new v3();
    vel.sub(vel1, vel2);
    float rel_vel;
    rel_vel = normal.dot(vel);
    for (int i = 0; i < 3; i++) {
        VectorUtil.setCoord(normal, i, 1f);
        float jacDiagABInv = 1f / jac[i].Adiag;
        /*
			//velocity error (first order error)
			btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
			m_rbB.getLinearVelocity(),angvelB);
			 */
        // positional error (zeroth order error)
        tmp.sub(pivotAInW, pivotBInW);
        // this is the error projected on the normal
        float depth = -tmp.dot(normal);
        float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv;
        float impulseClamp = this.impulseClamp;
        if (impulseClamp > 0f) {
            if (impulse < -impulseClamp) {
                impulse = -impulseClamp;
            }
            if (impulse > impulseClamp) {
                impulse = impulseClamp;
            }
        }
        appliedImpulse += impulse;
        v3 impulse_vector = new v3();
        impulse_vector.scale(impulse, normal);
        tmp.sub(pivotAInW, centerOfMassA);
        rbA.impulse(impulse_vector, tmp);
        tmp.negate(impulse_vector);
        tmp2.sub(pivotBInW, centerOfMassB);
        rbB.impulse(tmp, tmp2);
        VectorUtil.setCoord(normal, i, 0f);
    }
}

Example 15 with Transform

use of spacegraph.space3d.phys.math.Transform in project narchy by automenta.

the class ConvexConcaveCollisionAlgorithm method calculateTimeOfImpact.

@Override
public float calculateTimeOfImpact(Collidable body0, Collidable body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
    v3 tmp = new v3();
    Collidable convexbody = isSwapped ? body1 : body0;
    Collidable triBody = isSwapped ? body0 : body1;
    // quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
    // only perform CCD above a certain threshold, this prevents blocking on the long run
    // because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
    tmp.sub(convexbody.getInterpolationWorldTransform(new Transform()), convexbody.getWorldTransform(new Transform()));
    float squareMot0 = tmp.lengthSquared();
    if (squareMot0 < convexbody.getCcdSquareMotionThreshold()) {
        return 1f;
    }
    Transform tmpTrans = new Transform();
    // const btVector3& from = convexbody->m_worldTransform.getOrigin();
    // btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
    // todo: only do if the motion exceeds the 'radius'
    Transform triInv = triBody.getWorldTransform(new Transform());
    triInv.inverse();
    Transform convexFromLocal = new Transform();
    convexFromLocal.mul(triInv, convexbody.getWorldTransform(tmpTrans));
    Transform convexToLocal = new Transform();
    convexToLocal.mul(triInv, convexbody.getInterpolationWorldTransform(tmpTrans));
    if (triBody.shape().isConcave()) {
        v3 rayAabbMin = new v3(convexFromLocal);
        VectorUtil.setMin(rayAabbMin, convexToLocal);
        v3 rayAabbMax = new v3(convexFromLocal);
        VectorUtil.setMax(rayAabbMax, convexToLocal);
        float ccdRadius0 = convexbody.getCcdSweptSphereRadius();
        tmp.set(ccdRadius0, ccdRadius0, ccdRadius0);
        rayAabbMin.sub(tmp);
        rayAabbMax.add(tmp);
        // is this available?
        float curHitFraction = 1f;
        LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, convexbody.getCcdSweptSphereRadius(), curHitFraction);
        raycastCallback.hitFraction = convexbody.getHitFraction();
        Collidable concavebody = triBody;
        ConcaveShape triangleMesh = (ConcaveShape) concavebody.shape();
        if (triangleMesh != null) {
            triangleMesh.processAllTriangles(raycastCallback, rayAabbMin, rayAabbMax);
        }
        if (raycastCallback.hitFraction < convexbody.getHitFraction()) {
            convexbody.setHitFraction(raycastCallback.hitFraction);
            return raycastCallback.hitFraction;
        }
    }
    return 1f;
}