|
| 1 | + |
| 2 | +""" |
| 3 | + result = ContactResult(; object1, object2, objectCoordinateRef) |
| 4 | +
|
| 5 | +Return a `result` providing results of elastic MPR contacts between |
| 6 | +`object1::`[`Object3D`](@ref) and `object2::`[`Object3D`](@ref). |
| 7 | +If `objectCoordinateRef::`[`Object3D`](@ref) is defined, all vector |
| 8 | +results are resolved in `objectCoordinateRef`, otherwise in world. |
| 9 | +
|
| 10 | +# Results |
| 11 | +
|
| 12 | +- `penetration` is the normal contact penetration (positive in case of contact). |
| 13 | +- `penetrationVelocity` is the normal contact penetration velocity (positive for compression). |
| 14 | +- `tangentialVelocity` is the absolute value of the tangential relative velocity. |
| 15 | +- `angularVelocity` it the absolute value of the relative angular velocity. |
| 16 | +- `normalForce` is the normal contact force (positive for pressure). |
| 17 | +- `tangentialForce` is the absolute value of the tangential contact force. |
| 18 | +- `torque` is the absolute value of the contact torque. |
| 19 | +- `positionVector` is the absolute position vector of the contact point, resolved in `objectCoordinateRef`. |
| 20 | +- `normalVector` is the unit vector in contact normal direction, pointing into `object1`, resolved in `objectCoordinateRef`. |
| 21 | +- `forceVector` is the total contact force vector acting at the contact point on object1, resolved in `objectCoordinateRef`. |
| 22 | + On `object2` the same force vector is applied in inverse direction. |
| 23 | +- `torqueVector` is the total contact torque vector acting at the contact point on object1, resolved in `objectCoordinateRef`. |
| 24 | + On `object2` the same torque vector is applied in inverse direction. |
| 25 | +""" |
| 26 | +mutable struct ContactResult{F <: Modia3D.VarFloatType} <: Modia3D.AbstractResultElement |
| 27 | + |
| 28 | + path::String |
| 29 | + |
| 30 | + object1::Object3D{F} |
| 31 | + object2::Object3D{F} |
| 32 | + objectCoordinateRef::Union{Object3D{F}, Nothing} |
| 33 | + |
| 34 | + penetrationResultIndex::Int |
| 35 | + penetrationVelocityResultIndex::Int |
| 36 | + tangentialVelocityResultIndex::Int |
| 37 | + angularVelocityResultIndex::Int |
| 38 | + normalForceResultIndex::Int |
| 39 | + tangentialForceResultIndex::Int |
| 40 | + torqueResultIndex::Int |
| 41 | + positionVectorResultIndex::Int |
| 42 | + normalVectorResultIndex::Int |
| 43 | + forceVectorResultIndex::Int |
| 44 | + torqueVectorResultIndex::Int |
| 45 | + |
| 46 | + function ContactResult{F}(; path::String = "", |
| 47 | + object1::Object3D{F}, |
| 48 | + object2::Object3D{F}, |
| 49 | + objectCoordinateRef::Union{Object3D{F}, Nothing}=nothing ) where F <: Modia3D.VarFloatType |
| 50 | + return new(path, object1, object2, objectCoordinateRef) |
| 51 | + end |
| 52 | +end |
| 53 | +ContactResult(; kwargs...) = ContactResult{Float64}(; kwargs...) |
| 54 | + |
| 55 | + |
| 56 | +function initializeResultElement(model::Modia.InstantiatedModel{F,TimeType}, result::ContactResult{F}) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} |
| 57 | + |
| 58 | + result.penetrationResultIndex = Modia.new_w_segmented_variable!(model, result.path*".penetration" , F(0), "m") |
| 59 | + result.penetrationVelocityResultIndex = Modia.new_w_segmented_variable!(model, result.path*".penetrationVelocity", F(0), "m/s") |
| 60 | + result.tangentialVelocityResultIndex = Modia.new_w_segmented_variable!(model, result.path*".tangentialVelocity" , F(0), "m/s") |
| 61 | + result.angularVelocityResultIndex = Modia.new_w_segmented_variable!(model, result.path*".angularVelocity" , F(0), "rad/s") |
| 62 | + result.normalForceResultIndex = Modia.new_w_segmented_variable!(model, result.path*".normalForce" , F(0), "N") |
| 63 | + result.tangentialForceResultIndex = Modia.new_w_segmented_variable!(model, result.path*".tangentialForce" , F(0), "N") |
| 64 | + result.torqueResultIndex = Modia.new_w_segmented_variable!(model, result.path*".torque" , F(0), "N*m") |
| 65 | + result.positionVectorResultIndex = Modia.new_w_segmented_variable!(model, result.path*".positionVector" , SVector{3,F}(0, 0, 0), "m") |
| 66 | + result.normalVectorResultIndex = Modia.new_w_segmented_variable!(model, result.path*".normalVector" , SVector{3,F}(0, 0, 0)) |
| 67 | + result.forceVectorResultIndex = Modia.new_w_segmented_variable!(model, result.path*".forceVector" , SVector{3,F}(0, 0, 0), "N") |
| 68 | + result.torqueVectorResultIndex = Modia.new_w_segmented_variable!(model, result.path*".torqueVector" , SVector{3,F}(0, 0, 0), "N*m") |
| 69 | + |
| 70 | + return nothing |
| 71 | +end |
| 72 | + |
| 73 | +function evaluateResultElement(model::Modia.InstantiatedModel{F,TimeType}, scene::Modia3D.Composition.Scene{F}, result::ContactResult{F}, time::TimeType) where {F <: Modia3D.VarFloatType, TimeType <: AbstractFloat} |
| 74 | + |
| 75 | + (contactPair, converse) = getElasticContactPair(scene, result.object1, result.object2) |
| 76 | + if !isnothing(contactPair) |
| 77 | + penetration = contactPair.results.penetration |
| 78 | + penetrationVelocity = contactPair.results.penetrationVelocity |
| 79 | + tangentialVelocity = contactPair.results.tangentialVelocity |
| 80 | + angularVelocity = contactPair.results.angularVelocity |
| 81 | + normalForce = contactPair.results.normalForce |
| 82 | + tangentialForce = contactPair.results.tangentialForce |
| 83 | + torque = contactPair.results.torque |
| 84 | + positionVector = contactPair.results.positionVector |
| 85 | + normalVector = contactPair.results.normalVector |
| 86 | + forceVector = contactPair.results.forceVector |
| 87 | + torqueVector = contactPair.results.torqueVector |
| 88 | + if converse |
| 89 | + normalVector = -normalVector |
| 90 | + forceVector = -forceVector |
| 91 | + torqueVector = -torqueVector |
| 92 | + end |
| 93 | + if !isnothing(result.objectCoordinateRef) |
| 94 | + positionVector = result.objectCoordinateRef.R_abs * positionVector |
| 95 | + normalVector = result.objectCoordinateRef.R_abs * normalVector |
| 96 | + forceVector = result.objectCoordinateRef.R_abs * forceVector |
| 97 | + torqueVector = result.objectCoordinateRef.R_abs * torqueVector |
| 98 | + end |
| 99 | + else |
| 100 | + penetration = F(0) |
| 101 | + penetrationVelocity = F(0) |
| 102 | + tangentialVelocity = F(0) |
| 103 | + angularVelocity = F(0) |
| 104 | + normalForce = F(0) |
| 105 | + tangentialForce = F(0) |
| 106 | + torque = F(0) |
| 107 | + positionVector = SVector{3,F}(0, 0, 0) |
| 108 | + normalVector = SVector{3,F}(0, 0, 0) |
| 109 | + forceVector = SVector{3,F}(0, 0, 0) |
| 110 | + torqueVector = SVector{3,F}(0, 0, 0) |
| 111 | + end |
| 112 | + |
| 113 | + if Modia.storeResults(model) |
| 114 | + Modia.copy_w_segmented_value_to_result(model, result.penetrationResultIndex, penetration) |
| 115 | + Modia.copy_w_segmented_value_to_result(model, result.penetrationVelocityResultIndex, penetrationVelocity) |
| 116 | + Modia.copy_w_segmented_value_to_result(model, result.tangentialVelocityResultIndex, tangentialVelocity) |
| 117 | + Modia.copy_w_segmented_value_to_result(model, result.angularVelocityResultIndex, angularVelocity) |
| 118 | + Modia.copy_w_segmented_value_to_result(model, result.normalForceResultIndex, normalForce) |
| 119 | + Modia.copy_w_segmented_value_to_result(model, result.tangentialForceResultIndex, tangentialForce) |
| 120 | + Modia.copy_w_segmented_value_to_result(model, result.torqueResultIndex, torque) |
| 121 | + Modia.copy_w_segmented_value_to_result(model, result.positionVectorResultIndex, positionVector) |
| 122 | + Modia.copy_w_segmented_value_to_result(model, result.normalVectorResultIndex, normalVector) |
| 123 | + Modia.copy_w_segmented_value_to_result(model, result.forceVectorResultIndex, forceVector) |
| 124 | + Modia.copy_w_segmented_value_to_result(model, result.torqueVectorResultIndex, torqueVector) |
| 125 | + end |
| 126 | + |
| 127 | + return nothing |
| 128 | +end |
| 129 | + |
| 130 | +function terminateResultElement(result::ContactResult{F}) where F <: Modia3D.VarFloatType |
| 131 | + return nothing |
| 132 | +end |
0 commit comments